Control device for mobile body

ABSTRACT

Ground contact portions are categorized in a tree structure manner such that all of the ground contact portions of a mobile body (mobile robot) equipped with three or more ground contact portions become leaf nodes and that an intermediate node exists between the leaf nodes and a root node having all the leaf nodes as its descendant nodes. On each node (a C-th node) having child nodes, the correction amounts of the desired relative heights of the ground contact portions of the C-th node are determined such that at least the difference between an actual posture inclination and a desired posture inclination of a predetermined portion, such as a base body, (posture inclination difference) is approximated to zero, and joints of the mobile body  1  are operated so that a desired relative height obtained by combining the correction amounts is satisfied.

TECHNICAL FIELD

The present invention relates to the control of a floor reaction force of a mobile body, such as a legged mobile robot and a wheeled mobile body having an active suspension. More specifically, the present invention relates to a device for properly controlling a floor reaction force to minimize influences of an unknown slope, projection or depression of a floor (ground surface) with which a mobile body, such as the legged mobile robot or the wheeled mobile robot having an active suspension, comes in contact. Moreover, the present invention relates to a device for properly controlling a floor reaction force while estimating the configuration of a floor.

BACKGROUND ART

Japanese Unexamined Patent Application Publication No. H5-305585 by the present applicant (Patent Document 1) discloses a technology of the compliance control of both legs of a two-legged mobile robot. This technology is applicable also to a robot having, for example, four legs. In this case, according to the compliance control based on the technology disclosed in the publication, a virtual plane is inclined while all legs are in contact with the virtual plane, so that an undulation or tilt over a large area of the floor can be dealt with. According to the technology of the publication, however, there have been cases where a load (floor reaction force) is not properly distributed if a floor has a local tilt, projection or depression. As a result, poor contact with a surface sometimes occurred, causing a portion (foot) to be in contact therewith to shake or to be subjected to an undue landing impact, with consequent deteriorated properties of foot contact with the ground. This further led to a danger in that a robot easily slips or spins. Further, there have been cases where control fails to properly distribute a floor reaction force moment for restoring the posture of the robot to a desired posture to the distal portion of each leg, causing the posture of the robot to shake. There have been also cases where, if a certain portion of the robot starts vibrating, then it is inconveniently difficult to attenuate the vibration. Further, there have been cases where the failure of proper distribution control of a floor reaction force to the distal end of each leg results in excessive load on some actuators.

In addition, Japanese Unexamined Patent Application Publication No. H10-277969 (Patent Document 2) and Japanese Unexamined Patent Application Publication No. 2001-322076 (Patent Document 3) by the present applicant disclose a technology for composite-compliance control and a technology for estimating the configuration of a floor. These technologies make it possible to properly distribute a floor reaction force to the distal portion of each leg in a two-legged mobile robot and to compositively estimate the configuration of a floor, more specifically, compositively and accurately estimate the inclination of the ground contact surface of each leg distal portion and the height difference between ground contact surfaces at the same time. These technologies in the publications are, in principle, applicable to a multi-legged robot, such as a four-legged robot; however, no description has been given of a technology for specifically applying them.

Further, there has been known a technique for estimating the height of a floor at the time of landing by using feelers (load sensors or contact sensors) provided at the distal ends of the legs of the four-legged robot. However, this technique is a sequential technique and has been incapable of dealing with a case where a floor deforms after the landing, as with a soft ground. Especially when the main body of the robot is severely vibrating, such as when the robot is moving fast, the height of a floor cannot be correctly estimated, and there has been a danger in that load is not properly distributed to each leg.

There has been also known a technique whereby the compliance control of each leg of a robot is independently conducted for each leg. This technique, however, does not take into account the fact that the legs interfere with each other through the vibration of the body of the robot. In other words, the compliance control is individually carried out on each leg. For this reason, the compliance control of each leg does not act in harmony, making it impossible to properly distribute a floor reaction force to each leg.

The present invention has been made with a view of the background described above, and it is an object thereof to provide a device for properly controlling a floor reaction force to minimize influences of an unknown slope, projection or depression of a floor with which a mobile body, such as a legged mobile robot or a wheeled mobile body having an active suspension, comes in contact. Moreover, an object of the present invention is to provide a device for properly controlling a floor reaction force while accurately estimating the configuration of a floor.

DISCLOSURE OF INVENTION

According to a first invention of a control device for a mobile body in accordance with the present invention, a control device for a mobile body that controls the operation of the mobile body comprises a desired motion determining means for determining a desired motion of the mobile body, such as a mobile robot having three or more ground contact portions connected through the intermediary of a plurality of joints such that mutual relative heights can be manipulated, and an actual posture inclination detecting means for detecting or estimating the actual posture inclination of a predetermined part, such as a base body of the mobile body, the operation of the mobile body being controlled to make the motion of the mobile body follow a desired motion, and further comprises:

node operation controlling means that categorizes the ground contact portions in a tree structure manner such that all of the ground contact portions become leaf nodes, and an intermediate node exists between the leaf nodes and a root node having all the leaf nodes as descendant nodes,

determines, on each of C-th nodes, which is a node having a plurality of ground contact portions as descendant nodes, the correction amounts of desired relative heights of a plurality of ground contact portions belonging to the C-th node for each of the C-th nodes such that a posture inclination error is approximated to zero on the basis of at least the posture inclination error, which is the difference between the actual posture inclination and a desired posture inclination of the predetermined part in the desired motion, determines, on the basis of a correction amount obtained by combining the determined correction amounts of all the C-th nodes, a corrected desired motion by adding at least a first correction of the desired relative heights of the plurality of ground contact portions belonging to each C-th node to the desired motion, and operates the joints such that the corrected desired motion that has been determined is satisfied.

In the present invention, regarding each node having child nodes, “ground contact portions belonging to the node” means, more accurately, “belonging as descendant nodes to the node.” This applies not only to the first invention but applies any one of the inventions of the present application.

According to the first invention, three or more ground contact portions provided in a mobile body are sorted into a tree structure, and the correction amounts (in other words, manipulated variables) of desired relative heights of a plurality of ground contact portions belonging to the C-th node are determined for each C-th node so as to approximate a posture inclination error to zero on each of the C-th nodes having child nodes. Then, based on a correction amount obtained by combining those correction amounts of all the C-th nodes, a corrected desired motion is determined by adding a first correction of the desired relative heights of the plurality of ground contact portions belonging to each C-th node to the desired motion. Then, the joints of the mobile body are operated such that the determined corrected desired motion is satisfied. This makes it possible to adjust the relative heights of the ground contact portions so as to approximate the posture inclination error to zero while accomplishing mutual balance of the floor reaction forces of the plurality of ground contact portions, as compared with a case where the heights of individual ground contact portions are independently (separately) adjusted. As a result, even if a floor with which a mobile body is to be in contact has a locally unknown tilt, projection or depression, an influence thereof will be minimized, permitting enhanced stability of a posture of a robot to be achieved.

In the first invention described above, preferably, each node in the tree structure has a set weight, and regarding a B-th, which is each node in the tree structure, if the B-th node is the leaf node, then the height of a ground contact portion, which is the leaf node, is defined as the height of the B-th node, and if the B-th node has child nodes, then a weighted mean value, which uses the weight, of the heights of all child nodes of the B-th node is defined as the height of the B-th node, and

if the node operation controlling means determines the corrected desired motion for only one arbitrary node among the C-th nodes by adding a first correction of the desired relative heights of the plurality of ground contact portions belonging to that one node to the desired motion, then the desired height of that one node in the corrected desired motion is preferably maintained at a desired height in the desired motion (a second invention).

According to the second invention, the desired height of each C-th node takes the weighted mean value of the desired heights of the child nodes of the C-th node (the desired heights are determined on the basis of the desired heights of the ground contact portions belonging to the C-th node in the aforesaid desired motion). If the corrected desired motion is determined for only one arbitrary node among the C-th nodes by adding a first correction of the desired relative heights of the plurality of ground contact portions belonging to that one node to the desired motion of the aforesaid desired gait, then the first correction is added to a desired motion such that the aforesaid weighted mean value of the child nodes of the aforesaid one node will be the same in a corrected desired motion and a desired motion.

As a result, for each C-th node, the desired relative heights of the plurality of ground contact portions belonging to each C-th node can be corrected while achieving mutual balance among the floor reaction forces of the plurality of ground contact portions belonging to each C-th node. This eventually permits further improved stability of a posture of the robot.

In a third invention of the control device for a mobile body in accordance with the present invention, a control device for a mobile body that controls the operation of the mobile body is equipped with desired motion determining means for determining a desired motion of the mobile body, such as a mobile robot having three or more ground contact portions connected through the intermediary of a plurality of joints such that mutual relative heights can be adjusted, and actual posture inclination detecting means for detecting or estimating the actual posture inclination of a predetermined part, such as the base body of the mobile body, the operation of the mobile body being controlled to make the motion of the mobile body follow a desired motion,

when the ground contact portions are sorted into a tree structure such that each of the ground contact portions provides a leaf node, and an intermediate node exists between the leaf node and a root node having all leaf nodes as descendant nodes thereof, a weight is set for each node in the tree structure,

comprising a node operation controlling means that determines a corrected desired motion obtained by adding at least a first correction of desired relative heights of a plurality of ground contact portions belonging to the C-th nodes to the desired motion so as to approximate a posture inclination error to zero on the basis of the posture inclination error, which is the difference between at least the actual posture inclination and a desired posture inclination of the predetermined part in the desired motion, and then operates the joints such that the corrected desired motion that has been determined is satisfied,

wherein, regarding a B-th node, which is each node in the tree structure, if the B-th node is the leaf node, then the height of the ground contact portion, which is the leaf node, is defined as the height of the B-th node, and if the B-th node has a child node, then a weighted mean value, which uses the weight, of the heights of all child nodes of the B-th node, is defined as the height of the B-th node, and if the node operation controlling means determines the desired motion for only the C-th node by adding a first correction of the desired relative heights of the plurality of ground contact portions belonging to the C-th node to the desired motion, then the desired height of the C-th node in the corrected desired motion is maintained at a desired height in the desired motion.

According to the third invention, the desired height of the predetermined C-th node takes the weighted mean value of the desired heights of the child nodes of the C-th node (this is determined on the basis of the desired heights of the ground contact portions belonging to the C-th node in the desired motion). If the aforesaid corrected desired motion is determined for only the C-th node by adding a first correction of the desired relative heights of a plurality of ground contact portions belonging to the C-th node to the desired motion, then the first correction is added to a desired motion such that the aforesaid weighted mean value of the child nodes of the C-th node will be the same in a corrected desired motion and a desired motion.

As a result, according to the third invention, while achieving mutual balance among the floor reaction forces of the plurality of ground contact portions belonging to the C-th node, the relative heights of the ground contact portions can be adjusted so as to approximate the posture inclination error to zero. Thus, as with the first invention or the second invention, even if a floor with which a mobile body is to be in contact locally has an unknown inclination, projection or depression, the influences thereof will be minimized, permitting improved stability of a posture of a robot to be achieved.

In the second invention and the third invention described above, regarding each node having child nodes in the tree structure, the weights of all child nodes of each node are preferably set such that the total sum of the weights of all the child nodes will be one (a fourth invention).

Preferably, the second invention and the third invention described above is equipped with a means for variably setting the weights of the nodes, and if the leaf nodes corresponding to ground contact portions that are not in contact with the ground are defined as D-th nodes, then the means for setting the weights sets the weight of each D-node or the weight of at least one of ancestor nodes of the D-th node to zero (a fifth invention).

The fifth invention makes it possible to prevent the height of the ground contact portion not in contact with the ground, which cannot contribute to bringing the posture inclination error to zero, from being wastefully corrected. The fifth invention may be combined with the fourth invention.

In the first invention and the second invention, if the ground contact portions are connected to the base body of the mobile body in such a manner that they are movable with respect to the base body, then the node operation controlling means, when determining the corrected desired motion, preferably determines the corrected desired motion by further adding to the desired motion a correction of a desired posture of the base body to restrain slippage, such as twist, of the ground contact portions belonging to the C-th node on a floor surface (a sixth invention). This applies also to the third invention described above (a seventh invention).

If there are many ground contact portions connected to the base body of a mobile body, the degrees of freedom of joints of the mobile body will be excessively low, causing the corrections of the relative heights of the ground contact portions to be subject to mechanical restrictions. If the joints are forcibly operated to satisfy determined desired relative heights, then some ground contact portions may incur slippage, including twist. In such a situation wherein a slippage occurs, there is a danger in that the floor reaction forces of the ground contact portions become inappropriate and the stability of a posture of the robot is impaired. In such a case, as in the sixth invention and the seventh invention, rather than simply correcting the desired relative heights of the ground contact portions belonging to the C-th node, a correction of a desired posture of the base body is further added to a desired motion so as to determine the corrected desired motion, thus making it possible to restrain slippage, including twist. Hence, the ground contact portions will not be subjected to undue floor reaction forces, making it possible to secure stability of a posture of the mobile body by setting the floor reaction forces to be appropriate.

In the sixth invention and the seventh invention, more preferably, the node operation controlling means determines the corrected desired motion such that the horizontal position of the overall center-of-gravity of the mobile body in the corrected desired motion or the horizontal position of a predetermined representative point of the base body of the mobile body is substantially the same as the horizontal position in the desired motion (an eighth invention). The eighth invention makes it possible to prevent the overall center-of-gravity of the mobile body from being considerably shifted in the horizontal direction relative to a proper position in a desired gait when a corrected desired motion is determined by adding a correction of a desired posture of the base body to a desired motion. Hence, the relative heights of the ground contact portions of the C-th node and the posture of the base body can be corrected while maintaining proper stability of the mobile body.

The sixth invention is suited to a case where the mobile body is a robot equipped with a plurality of link mechanisms extended from its base body, and at least one link mechanism out of the plurality of link mechanisms has a joint provided at an intermediate portion between at least its distal portion and the end portion adjacent to the base body, the intermediate portion being the ground contact portion (a ninth invention).

The seventh invention is suited to a case where the mobile body is a robot equipped with a plurality of link mechanisms extended from its base body, and at least one link mechanism out of the plurality of link mechanisms has a joint provided at an intermediate portion between at least its distal portion and the end portion adjacent to the base body, the intermediate portion being the ground contact portion belonging to the C-th node (a tenth invention).

More specifically, a robot with a plurality of link mechanisms extended from its base body usually contacts the ground through the distal portions of the link mechanisms; therefore, an attempt to make the robot contact the ground at intermediate portions of the link mechanisms (e.g., an attempt to make a human type mobile robot contact the ground at an intermediate portion (knee) of a leg thereof) leads to the above-mentioned slippage, such as twist, due to an insufficient degree of freedom of a joint in some cases. Hence, the sixth invention or the seventh invention is suited to a case where a mobile body is constructed as described in the ninth invention or the tenth invention.

In these ninth invention and tenth invention, the intermediate portion is preferably provided with an elastic member that resiliently deforms when it comes in contact with the ground (an eleventh invention).

With this arrangement, the capability of adjusting a floor reaction force by correcting the relative heights of the intermediate portions is enhanced, and the stability of a posture of a mobile body (robot) can be easily secured.

Further, the sixth invention is suited also to a case where the mobile body is equipped with a plurality of link mechanisms which are extended from its base body and each of which has one or more joints, and at least one link mechanism among the plurality of link mechanisms and the base body are provided with the ground contact portions (a twelfth invention).

Further, the seventh invention is suited also to a case where the mobile body is equipped with a plurality of link mechanisms which are extended from its base body and each of which has one or more joints, at least one link mechanism among the plurality of link mechanisms and the base body are provided with the ground contact portions, and ground contact portions belonging to the C-th node include at least the ground contact portion of the base body (a thirteenth invention).

For instance, when making a robot that has one or more legs (link mechanisms) extended from a base body (a body or the like) sit down, the base body is to come in contact with the ground. And in such a case, an insufficient degree of freedom of joints occasionally results, causing the aforementioned slippage, such as twist, to take place. Thus, the sixth invention or the seventh invention is suited also to a case where a mobile body is constructed as described in the twelfth invention or the thirteenth invention.

In these twelfth invention and the thirteenth invention, preferably, at least one ground contact portion of the base body is provided with an elastic member that resiliently deforms when it comes in contact with the ground (a fourteenth invention).

With this arrangement, the capability of adjusting a floor reaction force by correcting the relative heights of the ground contact portions in the base body is enhanced, and the stability of a posture of a mobile body (robot) can be easily secured.

The sixth to the fourteenth inventions described above may be combined with the second invention, the fourth invention or the fifth invention described above.

Further, the first invention and the third invention described above (or the first to the fourteenth inventions) preferably comprises a means for determining a translational force component of a desired floor reaction force to be applied to ground contact portions belonging to the C-th node when the joint is operated to satisfy the corrected desired motion if the ground contact portions are connected to the base body through the intermediary of connecting mechanisms such that they are movable with respect to the base body of the mobile body,

wherein the node operation controlling means comprises a means for estimating a deformation amount that occurs at the connecting mechanisms between the base body and the ground contact portions belonging to the C-th node and the ground contact portions when the translational force component of the desired floor reaction force is applied to the ground contact portions belonging to the C-th node on the basis of at least the translational force components of the desired floor reaction forces of the ground contact portions belonging to the C-th node, and determines the corrected desired motion by further adding a second correction of the desired heights of the plurality of ground contact portions belonging to the C-th node, which is for canceling the estimated deformation amount, to the desired motion (a fifteenth invention).

According to the fifteenth invention, the occurrence of a deformation, such as a flexure, of the connecting mechanisms or the ground contact portions caused by a floor reaction force applied when the ground contact portions come in contact with the ground is taken into account, and the amount of the deformation is estimated on the basis of the translational force component of a desired floor reaction force. Then, the second correction of the desired heights of the ground contact portions belonging to the C-th node for canceling the estimated deformation amount is further added to the desired motion so as to determine the corrected desired motion. Thus, the fifteenth invention makes it possible to apply a floor reaction force that properly approximates the posture inclination error to zero to a mobile body while compensating for the influences of the deformations of the connecting mechanisms or the ground contact portions, allowing the stability of a posture of the mobile body to be further improved.

Preferably, the first invention and the third invention (or the first to the fifteenth inventions) comprises a floor configuration estimating means for estimating a parameter that specifies the relative heights of the ground contact surfaces of at least a plurality of ground contact portions belonging to the C-th node as the floor configuration parameter that represents a floor configuration,

wherein the node operation controlling means determines the corrected desired motion by further adding a third correction of the desired relative heights of a plurality of the ground contact portions belonging to the C-th node on the basis of an estimated value of a floor configuration parameter when determining the corrected desired motion (a sixteenth invention).

According to the sixteenth invention, a third correction of the desired relative heights of a plurality of the ground contact portions belonging to the C-th node on the basis of an estimated value of a floor configuration parameter that specifies at least the relative heights of the ground contact surfaces of a plurality of ground contact portions belonging to the C-th node is further added to a desired motion; therefore, even if an actual floor configuration differs from the one assumed in the desired motion, it is possible to make the ground contact surfaces of the ground contact portions properly come in contact with the actual floor surface when they are to be in contact with it and consequently to cause a floor reaction force that approximates the posture inclination error to zero to properly act on a mobile body. Hence, the stability of a posture of the mobile body can be further enhanced.

Preferably, the sixteenth invention comprises a floor reaction force detecting means for detecting or estimating an actual floor reaction force that acts on each of the ground contact portions,

wherein the floor configuration estimating means performs estimation while sequentially updating the floor configuration parameter on the basis of a past value of an estimated value of the floor configuration parameter, a difference between the corrected desired motion and the desired motion, the corrected motion, at least either a detected value or an estimated value of an actual joint displacement, which is a displacement amount of each joint of the mobile body, the actual posture inclination, and an actual floor reaction force of each of the ground contact portions belonging to at least the C-th node (a seventeenth invention).

Alternatively, the invention preferably comprises a floor reaction force detecting means for detecting or estimating an actual floor reaction force that acts on each of the ground contact portions, and

a means for sequentially determining at least an actual node floor reaction force, which is an actual value of the node floor reaction force of each child node of the C-th node, from the actual floor reaction force of each ground contact portion belonging to the C-th node if, regarding A-th nodes, which are the nodes in the tree structure, the A-th nodes are the leaf nodes, then the floor reaction forces acting on the ground contact portions, which are the leaf nodes, are defined as the node floor reaction forces of the A-th nodes, and if the A-th nodes have child nodes, then the resultant force of the node floor reaction forces of all the child nodes of the A-th nodes is defined as the node floor reaction force of the A-th nodes,

wherein the floor configuration estimating means performs estimation while sequentially updating the floor configuration parameter on the basis of the past value of an estimated value of the floor configuration parameter, the difference between the corrected desired motion and the desired motion, the corrected motion, at least either a detected value or an estimated value of an actual joint displacement, which is a displacement amount of each joint of the mobile body, the actual posture inclination, and a relative relationship among the actual node floor reaction forces of the child nodes of at least the C-th node (an eighteenth invention).

Alternatively, if the ground contact portions are connected to the base body through the intermediary of connecting mechanisms such that they are movable with respect to the base body of the mobile body,

the invention preferably comprises a deformation amount detecting means for detecting or estimating the amounts of deformations that occur at the connecting mechanisms between the base body and the ground contact portions and at the ground contact portions,

wherein the floor configuration estimating means performs estimation while sequentially updating the floor configuration parameter on the basis of the past value of an estimated value of the floor configuration parameter, a difference between the corrected desired motion and the desired motion, the corrected motion, at least either a detected value or an estimated value of an actual joint displacement, which is a displacement amount of each joint of the mobile body, the actual posture inclination, and the deformation amount associated with each ground contact portion belonging to at least the C-th node (a nineteenth invention).

According to the seventeenth invention, the eighteenth invention, and the nineteenth invention, floor configuration parameters can be properly estimated while sequentially updating them in real time. Supplementally, in the nineteenth invention, it is not necessarily required that all the connecting mechanisms be provided with joints.

Further, the nineteenth invention comprises, for example, a floor reaction force detecting means for detecting or estimating an actual floor reaction force acting on each of the ground contact portions, wherein the deformation amount detecting means estimates the deformation amount on the basis of the actual floor reaction force (a twentieth invention).

By detecting or estimating an actual floor reaction force as described above and using it, the deformation amounts of the connecting mechanisms or the ground contact portions can be properly estimated.

In the sixteenth invention (or the sixteenth to the twentieth inventions), each node in the tree structure has a set weight, and regarding a B-th node, which is a node in the tree structure, if the B-th node is the leaf node, then the height of the ground contact surface of the ground contact portion, which is the leaf node, is defined as the height of the ground contact surface of the B-th node, and if the B-th node has child nodes, then a weighted mean value, which uses the weight, of the heights of all child nodes of the B-th node is defined as the height of the ground contact portion of the B-th node, and when these definitions apply,

the floor configuration parameter estimated by the floor configuration estimating means is preferably a parameter that specifies a relative relationship of the heights of the ground contact surfaces of a plurality of child nodes of the C-th node (a twenty-first invention).

According to the twenty-first invention, defining the height of the ground contact surface of the B-th node having child nodes as the weighted mean value of the child nodes, as described above, makes it possible to determine a floor configuration parameter related to the configuration of a floor with which a plurality of ground contact portions belonging to the C-th node comes in contact by the relative relationship among the heights of the ground contact surfaces of the plurality of child nodes of the C-th node. Hence, floor configuration parameters can be estimated without grasping the absolute heights of the ground contact portions. This permits easier processing for estimating a floor configuration.

In the twenty-first invention, if at least one of the child nodes of the C-th node is about to float, the floor configuration estimating means preferably estimates the floor configuration parameter that specifies the relative relationship of the heights of the ground contact surfaces of the plurality of child nodes while retaining, at a fixed value, the relative height of the ground contact surface of the child node that is about to float (a twenty-second invention).

More specifically, the state in which at least one of the child nodes of the C-th node is about to float means that the ground contact surface of the child node has left or is about to leave an actual floor surface; therefore, the height of the ground contact surface of the child node does not reflect the height of the actual floor surface. Hence, according to the twenty-second invention, in such a state, the floor configuration parameter that specifies the relative relationship of the heights of the ground contact surfaces of the plurality of child nodes is estimated while holding, at a fixed value, the relative height of the ground contact surface of the child node that is about to float. This makes it possible to prevent a floor configuration parameter from diverging into a value that does not reflect an actual floor surface configuration, and to estimate a floor configuration parameter that reflects an actual floor surface configuration. Consequently, it is possible to prevent an inappropriate floor configuration parameter from being used to correct the relative heights of ground contact portions, which would impair the stability of a posture of a mobile body.

Further, in the sixteenth invention (or the sixteenth to the twenty-second inventions) described above, the floor configuration estimating means preferably estimates the floor configuration parameter by using a low-pass filter to alleviate fluctuation in the floor configuration parameter (a twenty-third invention). Using a low-pass filter like this makes it possible to prevent a floor configuration parameter from frequently varying due to a vibration or the like when a mobile body moves.

Further, in the sixteenth invention (or the sixteenth to the twenty-third inventions), a weight for estimating a floor configuration is set on each node of the tree structure, and

if processing in which, when a predetermined type of state amount, such as a height or a floor reaction force, is associated with each leaf node in the tree structure, the state amount of each node having child nodes is defined as a weighted mean value, which uses the weight, of the state amounts of all child nodes of the node, a value obtained by subtracting a state amount of a parent node of each node from the state amount of the node is determined as a node relative state amount of the node on the node except a root node, and zero is determined as a relative state amount of the root node, is defined as the processing for relatively hierarchizing the predetermined type of state amount on each node,

if a node relative floor reaction force Fn_rel(n=1,2, . . . ) hierarchically relativized on each node is determined on the basis of a floor reaction force Fn(n=1,2, . . . ) acting on each of the ground contact portions, which are the leaf nodes, and when a vector (Fa1_rel,Fa2_rel, . . . ,Far_rel) having the node relative floor reaction forces of all child nodes aj (j=1,2, . . . ,r. r denotes the total number of the child nodes of an n-th node) of an n-th node, which is an arbitrary node having child nodes, is represented by a linear linkage of a plurality of predetermined mutually independent vectors R(j) (j=1,2, . . . ,r-1) that are all orthogonal to a vector (Wa1,Wa2, . . . ,War) having the weights of all child nodes of the n-th node as its elements, a vector having a coefficient of the linear linkage as its element is defined as a node expansion floor reaction force moment Mn_exp of the n-th node, and

if a node relative height Zn_rel(n=1,2, . . . ) hierarchically relativized on each node is determined on the basis of a height Zn(n=1,2, . . . ) of the ground contact surface of each of the ground contact portions, which are the leaf nodes, and when a vector (Za1_rel, Za2_rel, . . . ,Zar_rel) having the node relative heights of all child nodes aj (j=1,2, . . . ,r. r denotes the total number of the child nodes of an n-th node) of the n-th node as its elements is expressed by a linear linkage of the plurality of the predetermined mutually independent vectors R(j) (j=1,2, . . . ,r-1), a vector having a coefficient of the linear linkage as its element is defined as a node expansion inclination angle θn of the n-th node,

then, the floor configuration estimating means may estimate the floor configuration parameter by using at least the node expansion floor reaction force moment Mn_exp(n=C) or the node expansion inclination angle θn (n=C) of at least the C-th node (a twenty-fourth invention).

Using the node expansion floor reaction force moment or the node expansion inclination angle defined as described above makes it possible to properly estimate a floor configuration parameter that represents a floor configuration even if there are four or more ground contact portions belonging to the C-th node.

In the twenty-fourth invention, regarding a B-th node, which is a node in the tree structure, if the B-th node is the leaf node, then the height of the ground contact surface of the ground contact portion, which is the leaf node, is defined as the height of the ground contact surface of the B-th node, and if the B-th node has child nodes, then a weighted mean value, which uses the weight, of the heights of the ground contact surfaces of all child nodes of the B-th node, is defined as the height of the ground contact surface of the B-th node, then

the floor configuration parameter preferably includes a parameter that uses the node expansion inclination angle to indicate the relative height of the ground contact surface of each child node of the C-th node (a twenty-fifth invention).

With this arrangement, a floor configuration parameter can be properly estimated by using the node expansion inclination angle even if there are four or more ground contact portions belonging to the C-th node.

Further, more specifically, the twenty-fourth invention comprises a floor reaction force detecting means for detecting or estimating an actual floor reaction force acting on each of the ground contact portions,

wherein the node operation controlling means comprises a means for sequentially determining the node expansion moment on the basis of at least an actual floor reaction force of each of the ground contact portions, and a means for sequentially determining the node expansion inclination angle by multiplying the determined node expansion moment by a predetermined matrix, and estimates a new floor configuration parameter on the basis of the determined node expansion inclination angle and the past value of an estimated value of the floor configuration parameter (a twenty-fifth invention).

According to the twenty-fifth invention, the node expansion moment is sequentially determined on the basis of at least the actual floor reaction force of each of the ground contact portions and the node expansion inclination angle is sequentially determined by multiplying the determined node expansion moment by a predetermined matrix; therefore, a new floor reaction force parameter can be determined while sequentially updating it on the basis of the determined node expansion inclination angle and the past value of an estimated value of the floor configuration parameter.

In the first to the twenty-fifth inventions described above, “floor reaction force” is to include a reaction force received from an object other than a floor with which a mobile body comes in contact rather than simply meaning a reaction force acting on the mobile body from a floor (or the ground), as in its original meaning. Further, the ground contact portion is to include not only a portion that comes in contact with a floor (or the ground) but also a portion of the mobile body that comes in contact with an object.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is an overall external view of a robot in accordance with a first embodiment and a second embodiment of the present invention, FIG. 2 is a block diagram showing the functional construction of a control device of the first embodiment, FIGS. 3(a) to (c) and FIGS. 4(a) to (c) are diagrams for explaining an operation of the robot (four-legged robot) of the first embodiment, FIGS. 5(a) to (c) are graphs showing setting examples of weights in the first embodiment, and FIG. 5(d) is a graph showing a setting example of a ZMP (desired total floor reaction force central point) in the first embodiment. FIG. 6 is a diagram showing the hierarchical structure of nodes in the first embodiment, FIG. 7 is a diagram showing a relationship between desired node floor reaction force central points and weights, FIG. 8 is a diagram showing a relationship between desired node floor reaction forces and weights, and FIG. 9 is a flowchart showing a main routine processing of the control device of the first embodiment. FIG. 10 to FIG. 14 are diagrams illustrating desired node floor reaction force translational force components, actual node floor reaction force translational force components, actual node floor reaction force moments, compensating total floor reaction force moments, and node compensating floor reaction force moments, respectively, of a robot (four-legged robot) of the first embodiment. FIG. 15 and FIG. 16 are diagrams for explaining position corrections by node compensating angles (compliance operation) in the first embodiment, FIG. 17 to FIG. 21 are block diagrams respectively showing the functional constructions of a hierarchical compliance operation determiner, a compensating total floor reaction force moment distributor, a θ1423 determiner (compensating angle determiner), a θ14 determiner (compensating angle determiner), and a mechanism deformation compensation amount calculator in the first embodiment, FIG. 22 is a flowchart showing the processing for determining compensating angles, and FIG. 23 is a diagram for explaining another example of a hierarchical structure related to the robot according to the first embodiment. FIG. 24 is a diagram for explaining the hierarchical structure of a robot (six-legged robot) according to a second embodiment, FIG. 25 is a block diagram showing the functional construction of a hierarchical compliance operation determiner in the second embodiment, FIG. 26 to FIG. 28 are diagrams respectively illustrating the translational force components of desired node floor reaction forces, the translational force components of actual node floor reaction forces, and actual node floor reaction force moments of the robot (six-legged robot) according to the second embodiment, FIGS. 29(a) and (b) are diagrams for explaining the technique for setting a node compensating floor reaction force moment in the second embodiment, and FIG. 30 and FIG. 31 are diagrams for explaining a position correction by a node compensating angle (compliance operation) in the second embodiment. FIG. 32 and FIG. 33 are block diagrams respectively showing the functions of a θ145236 determiner (compensating angle determiner) and a θ145 determiner (compensating angle determiner) in the second embodiment, and FIG. 34 is a diagram for explaining another example of a hierarchical structure related to the robot according to the second embodiment. FIG. 35 is a block diagram showing the functional construction of a hierarchical compliance operation determiner in a third embodiment, FIG. 36 is a flowchart showing a main routine processing of a control device in the third embodiment, FIG. 37 to FIG. 39 are diagrams for explaining concepts and terms in the third embodiment, FIG. 40 is a block diagram showing the function of a floor configuration estimator in the third embodiment, and FIG. 41 is a flowchart showing the processing of a floor height error estimation processing subroutine in FIG. 40. FIG. 42 and FIG. 43 are diagrams for explaining the processing for hierarchical relativization in a four-legged robot and a six-legged robot, respectively. FIG. 44 is a diagram showing examples of setting request modes of nodes related to estimating a floor configuration, and FIG. 45 to FIG. 55 are flowcharts showing the processing for estimating a floor configuration. FIG. 56 is a block diagram showing the functions of a floor configuration estimator in a fourth embodiment, and FIG. 57 is a block diagram showing the functions of a floor configuration estimator in a fifth embodiment. FIG. 58 and FIG. 59 are diagrams showing a state wherein a robot (two-legged mobile robot) in a sixth embodiment is on its knee in contact with a floor, FIG. 60 is a diagram showing the construction of a floor reaction force sensor of a knee of the robot in the sixth embodiment, FIG. 61 is a diagram showing a hierarchical structure of nodes in the sixth embodiment, and FIG. 62 is a block diagram showing the functional construction of a hierarchical compliance operation determiner in the sixth embodiment. FIGS. 63(a) to (c) are diagrams for explaining a technique for correcting the posture of the body of the robot in the sixth embodiment, FIG. 64 is a diagram for explaining a technique for correcting the position/posture of the body of the robot in the sixth embodiment, and FIG. 65 is a block diagram showing the functions of an inverse kinematics calculator in the sixth embodiment. FIG. 66 is a block diagram showing the functions of an inverse kinematics calculator in a seventh embodiment, and FIG. 67 and FIG. 68 are diagrams for explaining a technique for correcting the posture of a robot in the seventh embodiment. FIG. 69 is a diagram showing the construction of a robot in an eighth embodiment and FIG. 70 is a diagram showing a hierarchical structure of nodes in the eighth embodiment.

BEST MODE FOR CARRYING OUT THE INVENTION

First, major terms to be used in the explanation of the embodiments of the present invention will be explained.

In the technology of the composite-compliance control disclosed in Japanese Unexamined Patent Application Publication No. H10-277969 previously proposed by the present applicant, floor reaction forces are sorted for each leg on the assumption that floor reaction forces are received at the distal portion (foot) of a leg of a robot. In comparison therewith, according to the embodiments of the present description, situations are also considered in which floor reaction forces are received at portions other than the distal portions of legs, e.g., a situation in which a human type robot is on its knee, sitting on a chair, or has its arm in contact with the ground. For this reason, a portion of a legged mobile robot that comes in contact with a floor and receives a reaction force in a predetermined operation of the robot is referred to as “the ground contact portion.” In this case, “floor” does not simply refers to a floor (or the ground) in a conventional sense, but “floor” also includes an object with which the robot comes in contact and receives a reaction force during its movement, such as a chair (a chair on which the robot sits) fixed to the floor (or the ground). Thus, if, for example, a two-legged mobile robot sits on a chair or the like, then the waist of the robot will correspond to the ground contact portion. In a normal travel of a legged mobile robot (in the walking or the like of the two-legged mobile robot), the distal portion (foot or the like) of each leg will of course correspond to the ground contact portion.

To categorize (discriminate) ground contact portions, even if ground contact portions are distributed such that they are separated at a plurality of locations on the same link (a part corresponding to a single rigid body)(if a plurality of portions separated from each other on the same link is in contact with the ground), that is, even if a plurality of ground contact surfaces are not connected on the same link, these will be put together and defined as one ground contact portion. For instance, if the ground contact portion provided with four spike pins comes in contact with the ground through the intermediary of the spike pins, then the individual ground contact surfaces of the four spike pins will be put together and regarded as one ground contact portion. However, it is not always required to put the ground contact surfaces together into one.

An n-th ground contact portion is a designation for identifying the ground contact portion which has been categorized (discriminated) according to such a rule and to which an identification number n (n=1,2,3, . . . ) has been assigned.

A ground contact portion floor reaction force refers to a floor reaction force acting on the ground contact portion, and the floor reaction force acting on an n-th ground contact portion, in particular, is referred to as the n-th ground contact portion floor reaction force. The total floor reaction force is the resultant force of the floor reaction forces acting on all ground contact portions. The floor reaction force central point is the point of action at which the horizontal component of a moment generated by a floor reaction force at that point reaches zero.

A floor reaction force, such as the ground contact portion floor reaction force or a total floor reaction force, is usually expressed by a set of the point of action of a force and a translational force and moment applied to that point of action. For the same floor reaction force, there are a limitless number of sets of point of action, translational force and moment for expressing the floor reaction force. Among them, there is an expression of a floor reaction force that uses, as the point of action, a point at which a moment component (the horizontal component of a moment) except a component about a vertical axis (vertical component) reaches zero. The point of action in this expression is referred to as a floor reaction force central point in the present description. A floor reaction force central point may be defined as a point at which the floor surface parallel component (the component parallel to a floor surface) of a moment generated by a floor reaction force at that point reaches zero.

In the present description, “floor surface” may alternatively be a virtual floor surface (a floor surface assumed in a desired gait and it does not necessarily coincide with an actual floor surface) described in Japanese Unexamined Patent Application Publication No. H5-318340 previously proposed by the present applicant.

Supplementally, the floor reaction force central point of the ground contact portion is usually set on the ground contact surface thereof (the surface in contact with a floor) if the ground contact portion is in contact with the ground. Meanwhile, the ground contact portion floor reaction force while the ground contact portion is moving in the air is always zero, so that a moment horizontal component of the ground contact portion floor reaction force will be zero at any point of action. This means that the floor reaction force central point can be arbitrarily set. However, to smoothly control the operation of a robot, a desired floor reaction force central point desirably moves in continuity. Hence, according to the embodiments in the present description, a floor reaction force central point of the ground contact portion floor reaction force is defined as the point of action that moves together with the ground contact portion when the ground contact portion moves in the air.

According to the embodiments in the present description, in the compliance control operation in which the position/posture of each ground contact portion are corrected on the basis of at least an actual floor reaction force (a floor reaction force actually acting on a robot), the compliance control operation is not simply performed on each of ground contact portions irrelevantly (independently) from each other. The embodiments in the present description are characterized in that ground contact portion are sorted by a tree structure and operations for correcting the positions/postures of ground contact portions are determined on the basis of at least an actual floor reaction force acting on each group that has been sorted (the actual floor reaction forces or the resultant force thereof acting on all ground contact portions included in each group). Hereinafter, sorting by the tree structure may be referred to as “hierarchization.”

For the desired values of variables generated by a gait generating device of a legged mobile robot in the embodiments in the present description, “desired” will be attached to the beginning of the names of variables. For the values of the variables in an actual legged mobile robot (actually detected values or estimated values, because true values are unknown), “actual” will be attached to the beginning of the names of variables. The above “actual floor reaction force” is an example thereof.

The targets of the total floor reaction forces (the resultant force of the actual floor reaction forces acting on all ground contact portions of the robot) in the compliance control (floor reaction force control) to be discussed later will be referred to as desired total floor reaction forces. The point at which the moment horizontal component of a desired total floor reaction force reaches zero will be referred to as a desired total floor reaction force central point.

The point on a floor surface at which the moment horizontal component of the resultant force of an inertial force generated by a motion of a desired gait of a mobile robot (the motion of each portion of the robot in a desired gait) and the gravity acting on the robot becomes zero will be referred to as a desired ZMP. The motion of a desired gait is determined by the time series of the desired position/posture of each portion of the robot in the desired gait, so that the time series of the desired position/posture of the portion will be generically referred to as a motion or a desired motion of a desired gait. Supplementally, if these time series can be specified, then desired motions may be described by expressions that are different from the desired motions defined as described above. For example, a set of the time series of the desired displacements of joints of a robot and the time series of the desired position/posture of a particular portion, such as a base body, may be used as a desired motion.

A desired total floor reaction force is usually a total floor reaction force that dynamically balances with the motion patter of a desired gait (the time-series pattern of a desired motion). Hence, a desired total floor reaction force central point usually agrees with a desired ZMP. Thus, in the following explanation, a desired total floor reaction force central point and a desired ZMP will be used without discriminating them in many cases. Exceptionally, however, in the case of controlling a robot explained in, for example, U.S. Pat. No. 3,269,852 previously proposed by the present applicant, a desired total floor reaction force central point and a desired ZMP do not necessarily agree with each other. In the following explanation, the term, desired ZMP, will be used in some cases, but there will be some places where the term, desired total floor reaction force central point, should be used to be precise.

It may be assumed that, in a desired gait of a mobile robot, the robot is subject to a reaction force (an external force) other than a floor reaction force from an environment. And, the reaction force (the external force) other than a floor reaction force may be referred to as, for example, a desired object reaction force, and the definition of a desired ZMP described above may be expanded as follows. The resultant force of the inertial force generated by a motion pattern of a desired gait of a robot, the gravity acting on the robot, and a desired object reaction force may be dynamically determined, and if the moment generated at a certain point on a floor surface by the resultant force is zero except for a component about a vertical axis, then the point may be defined anew as a desired ZMP. However, if a desired object reaction force is taken as one form of a floor reaction force, then the definition of a desired ZMP may be the same definition previously described.

Based on the above, the embodiments of the present invention will be explained in detail below.

First Embodiment

FIG. 1 is an external view of a general multi-legged mobile robot (legged mobile robot) according to first and second embodiments. FIG. 1 shows that the robot 1 has six legs (leg bodies), namely, a first leg #1 to a sixth leg #6; however, it does not have the fifth leg #5 and the sixth leg #6 in the first embodiment. This means that, in the first embodiment, the robot 1 is a four-legged robot having four legs (leg bodies), the first leg #1 to the fourth leg #4. In FIG. 1, the components of the robot 1 according to the second embodiment are shown by parenthesized reference numerals.

As shown in FIG. 1, in the robot 1 (the four-legged robot) according to the first embodiment, two legs (the first leg #1 and the third leg #3) are extended from the right side of a body 24, which is the base body of the robot 1, such that they are arranged side by side in the longitudinal direction, and in the same manner, two legs (the second leg #2 and the fourth leg #4) are extended from the left side of the body 24 such that they are arranged side by side in the longitudinal direction. A discoid ground contact portion 10 is attached to the distal portion of each of the legs #1 to #4 through the intermediary of a spherical joint 12, which is a free joint. Hereinafter, when the ground contact portions 10 need to be discriminated among the legs #1 to #4, they will be referred to as the first ground contact portion, the second ground contact portion, the third ground contact portion, and the fourth ground contact portion.

Each ground contact portion 10 is engaged with the spherical joint 12 such that its central point agrees with the central point of the spherical joint 12 and that a floor reaction force moment (the moment component of a floor reaction force) will not act on the central point of the ground contact portion 10 (strictly speaking, the spherical joint 12). This means that the floor reaction force moment (actual floor reaction force moment) at the central point of the ground contact portion 10 will be always zero.

In the robot 1 shown in the figure, each of the legs #1 to #4 is provided with joints 14 and 15 at a portion adjacent to the body 24 of the robot 1 and at an intermediate portion, respectively, and a compliance mechanism 42 composed of an elastic member, such as a spring, and a six-axis force sensor 34 serving as a floor reaction force detecting means for detecting an actual floor reaction force acting on the ground contact portion 10 are provided in the vicinity of the distal portion of each of the legs #1 to #4 (a portion of the link connecting the spherical joint 12 and the joint 14, the portion being adjacent to the spherical joint 12). In the example shown in the figure, the joints 14 permit rotation about two axes, while the joints 15 permit rotation about one axis. The bottom surfaces of the ground contact portions may be provided with elastic members made of sponge, rubber or the like serving as compliance mechanisms.

The displacement operation (the rotational operation about each axis) of each of the joints 14 and 15 is performed by an actuator, such as an electric motor, which is not shown. And, an actual joint displacement, which is the actual displacement amount (the angle of rotation about each axis), of each of the joints 14 and 15 is detected by a sensor, such as a rotary encoder, which is not shown. The six-axis force sensor 34 is capable of detecting the translational forces in the directions of three axes and the moments about three axes; however, in the robot 1 in the first embodiment, no actual floor reaction force moment acts on the central points of the ground contact portions 10, as described above. Hence, three-axis force sensors that detect the translational forces in the directions of three axes or force sensors that detect only the vertical components of the translational forces of actual floor reaction forces may be used in place of the six-axis force sensors 34.

The body 24 incorporates a control device 50 constructed of an electronic circuit unit that includes a microcomputer, an actuator drive circuit, etc., a posture sensor 36 for detecting the postures of the body 24, and a power source not shown (a secondary battery, a capacitor, or the like). The posture sensor 36 is constructed of, for example, an acceleration sensor and a gyro sensor. In the present description, “posture” generally means a spatial orientation (however, “the posture” of the entire robot means an instantaneous value of a motion of the robot). Further, in the present embodiment, the posture sensor 36 detects the posture inclinations (inclination angles) in, for example, the pitch directions and roll directions in relation to the vertical direction, of the postures of the body 24. In other words, the posture sensor 36 functions as an actual posture inclination detecting means for detecting actual posture inclinations of the body 24.

If the postures (rotational angles) of the body 24 of the robot 1 also in the yaw direction are to be controlled, then the rotational angles of the body 24 in the yaw direction (in other words, the postures of the body 24 in the directions of three axes) may be also detected by the posture sensor 36.

FIG. 2 is a block diagram showing the functional construction and operations of the control device 50. The actual robot 1 shown in FIG. 2 is the robot 1 shown in FIG. 1 from which the control device 50, the posture sensor 36, and the six-axis force sensor 34 have been removed. Here, a predetermined coordinate system (XYZ coordinate system) fixed to a floor, in which the approximately front direction of the robot 1 is defined as X axis, approximately left side direction thereof is defined as Y axis, and the upward direction thereof is defined as Z axis, as shown in FIG. 1, is referred to as “a supporting leg coordinate system” or “a global coordinate system.” Hereinafter, positions, postures, translational forces and moments will be expressed in terms of coordinate components of the supporting leg coordinate system (the global coordinate system) unless otherwise specified. The origin of the supporting leg coordinate system (global coordinate system) does not have to be steadily fixed at a single point; the position of the origin with respect to a floor may be changed while the robot 1 is traveling. For example, the position of the origin of the supporting leg coordinate system (the global coordinate system) may be changed each time a predetermined leg of the robot 1 lands.

As shown in FIG. 2, the control device 50 is provided with a gait generating device 100, a desired floor reaction force distributor 102, a robot geometric model (inverse kinematics calculator) 110, a hierarchical compliance operation determiner 114, a displacement controller 112, an actual floor reaction force detector 108, a posture error calculator 103, and a posture stabilization control calculator 104 as its functional components (functional means). The following will explain the overviews of the components of the control device 50.

The gait generating device 100 has a function as a desired gait determining means or a desired motion determining means in the present invention, and generates (determines) and outputs a desired gait that specifies an operation of the robot 1. In the present embodiment, a desired gait is formed of the trajectory of a desired motion of the robot (the time series of a desired position/posture of each portion of the robot) and the trajectory of a desired floor reaction force (the time series of a set of the desired position of the action point of a reaction force received by the robot from a floor and the desired values of a translational force and moment applied to the action point). In the present description, “trajectory” means a time-series pattern (temporal change pattern).

The trajectory of a desired motion output by the gait generating device 100 is constructed of a desired ground contact portion trajectory, which is the trajectory of the desired values of the position and the posture of each ground contact portion 10 of the robot 1, and a desired body position/posture trajectory, which is the trajectory of the desired values of the position and the posture of the body 24 of the robot 1. A gait generating device in a robot equipped with joints related to arms or a head, as in a sixth embodiment to be discussed later, determines and outputs the desired position/posture trajectories of the arms or the head as constituents of desired motions.

Further, in the present embodiment, the trajectory of a desired floor reaction force output by the gait generating device 100 is formed of a desired total floor reaction force central point trajectory, which is the trajectory of a desired position of the total floor reaction force central point of the robot 1, and a desired total floor reaction force trajectory, which is the trajectory of a desired value of a total floor reaction force that uses the above desired total floor reaction force central point as an action point. In the present embodiment, a desired total floor reaction force central point trajectory is regarded as identical to a desired ZMP trajectory, which is the trajectory of a desired position of ZMP.

The position of each ground contact portion 10 is the position of a certain representative point of the ground contact portion 10, and the representative point is, for example, the projected point obtained by projecting, in the vertical direction, the central point of each ground contact portion 10 (the central point of the spherical joint 12) onto the ground contact surface (bottom surface) of the ground contact portion 10, or the central point of the ground contact portion 10 (the central point of the spherical joint 12). Hereinafter, the position of the representative point of each ground contact portion 10 will be referred to simply as the ground contact portion position. The trajectory of a desired value of the ground contact portion position (a desired ground contact portion position trajectory) is included in the aforesaid desired ground contact portion trajectory determined by the gait generating device 100.

Here, in the robot 1 of the present embodiment, the ground contact portions 10 are engaged with the spherical joints 12, which are free joints, so that the postures of the ground contact portions 10 cannot be controlled. Therefore, according to the present embodiment, the gait generating device 100 does not actually generate the trajectories of desired postures of the ground contact portions 10 (does not have to generate them). In the present embodiment, therefore, the desired ground contact portion trajectory means the same as a desired ground contact portion position trajectory.

However, if the ground contact portions are attached to the distal portions of the legs such that their postures can be controlled (if the ground contact portions are attached to the distal portions of the legs through joints that can be operated by actuators), as in the robot of the sixth embodiment to be discussed later, then the trajectories of desired postures of the ground contact portions should be included in the desired ground contact portion trajectories. In the present description, the term “ground contact portion position/posture” will be frequently used to generally consider the aforesaid case, the term substantially meaning “ground contact portion position” in the present embodiment.

Referring to FIG. 3(a) to FIG. 3(c) and FIG. 4(a) to FIG. 4(c), a desired ground contact portion trajectory (a desired ground contact portion position trajectory) and a desired total floor reaction force central point trajectory will be explained more specifically. According to the first embodiment, a travel of the robot 1 is accomplished by carrying out the motions of the legs #1 to #4 by causing a pair of legs, which are to be free legs, to leave a floor and move in the air and then land at a desired position by repeating, in order, a period during which the pair of the first leg #1 and the fourth leg #4 out of the legs #1 to #4 is a pair of supporting leg, while the pair of the second leg #2 and the third leg #3 is a pair of free legs, a period during which all legs #1 to #4 are supporting legs, and a period during which the pair of the first leg #1 and the fourth leg #4 is a pair of free legs, while the pair of the second leg #2 and the third leg #3 is a pair of supporting legs. A supporting leg is a leg that is in contact with the ground to support the self-weight of the robot 1 (a leg to be subject to a floor reaction force that is not zero), and a free leg is a non-supporting leg.

FIG. 3(a) to FIG. 3(c) and FIG. 4(a) to FIG. 4(c) show the desired ground contact portion positions (more specifically, the positions on a horizontal plane (XY plane)) of the distal ends of the legs #1 to #4 during the aforesaid travel of the robot 1 in time series in sequence (in the sequence of time t1 to t6). The triangles corresponding to reference characters Q1 to Q4 in these figures indicate the desired ground contact portion positions of the first to the fourth ground contact portions 10, respectively (the positions of the aforesaid representative points of the first to the fourth ground contact portions 10 on a horizontal plane (XY plane)).

Supplementally, to be precise, the triangles with the reference characters Q1 to Q4 in FIG. 3 and FIG. 4 indicate the positions of desired node floor reaction force central points (desired ground contact portion floor reaction force central points) to be discussed later, which are related to the ground contact portions 10. In the present embodiment, however, the representative points of the ground contact portions 10 are set as described above, so that the desired ground contact portion positions (the desired positions of the representative points) of the first to the fourth ground contact portions 10 agree with the positions of the desired node floor reaction force central points Qn (n=1,2,3,4), which will be discussed later, or have a certain positional relationship with the positions of the respective desired node floor reaction force central points Qn (n=1,2,3,4) (the positions on the horizontal plane (XY plane) agree). For this reason, the triangles corresponding to the reference characters Q1, Q2, Q3 and Q4 in FIG. 3 and FIG. 4 denote the desired node floor reaction force central points, which will be discussed later, of the first to the fourth ground contact portions 10, respectively, and also denote the positions of the first to the fourth ground contact portions 10, respectively.

FIG. 3(a) shows the desired ground contact portion positions of the ground contact portions 10 at the instant (time t1) when the pair of the first leg #1 and the fourth leg #4 as free legs are landed, the pair of the second leg #2 and the third leg #3 being supporting legs, FIG. 3(b) shows the desired ground contact portion positions at time t2 in a state wherein all the legs #1 to #4 are supporting legs, and FIG. 3(c) shows the desired ground contact portion positions at time t3 immediately before the pair of the second leg #2 and the third leg #3 as free legs is separated from a floor (lifted into the air), the pair of the first leg #1 and the fourth leg #4 being supporting legs. Further, FIG. 4(a) shows the desired ground contact portion positions at time t4 in a state wherein the pair of the second leg #2 and the third leg #3 as free legs is lifted into the air, the pair of the first leg #1 and the fourth leg #4 being supporting legs, FIG. 4(b) shows the desired ground contact portion positions at the instant (time t5) when the pair of the second leg #2 and the third leg #3 as free legs are landed, the pair of the first leg #1 and the fourth leg #4 being supporting legs, and FIG. 4(c) shows the desired ground contact portion positions at time t6 immediately before the pair of the first leg #1 and the fourth leg #4 as free legs is separated from a floor, the pair of the second leg #2 and the third leg #3 being supporting legs. Incidentally, in FIG. 4(a), the desired ground contact portion positions of the second leg #2 and the third leg #3, which are free legs, are indicated by dashed-line triangles. Supplementally, the trajectory of the positions of the ground contact portions 10 of free legs in the vertical direction (the Z-axis direction) are determined such that they rise from a floor surface to a predetermined height and then lower to land again.

Point P in these FIG. 3 and FIG. 4 indicate desired total floor reaction force central points (desired ZMPs). The desired total floor reaction force central point trajectories are determined such that they continuously move while existing in a range in which ZMPs may exist (a region on a floor surface corresponding to a so-called supporting polygon) at positions not excessively close to the boundaries of the range (e.g., at an approximately central position of the range wherein a ZMP may exist). In the first embodiment, when two legs #1 and #4 or #2 and #3 are supporting legs (refer to FIGS. 3(a) and (c) and FIGS. 4(a) to (c)), a desired total floor reaction force central point is set on a segment connecting the representative points of the ground contact portions 10 and 10 of those legs such that it is not excessively close to an end point of the segment. When all legs #1 to #4 are supporting legs (refer to FIG. 3(b)), a desired total floor reaction force central point is set within a polygon having the representative points of all ground contact portions 10 as its apex angles such that it is not excessively close to a boundary of the polygon. FIG. 5(d) is a graph illustrating a trajectory of a component ZMPx in the X-axis direction (in the direction in which the robot 1 advances) of the position of the desired total floor reaction force central point (desired ZMP) determined as described above. FIGS. 5(a) to (c) are graphs illustrating setting examples of weight, which will be discussed later.

Further, a desired body position/posture trajectory determined by the gait generating device 100 is determined using a dynamic model or the like of the robot 1 such that the horizontal component of a moment generated about a desired ZMP by the resultant force of the inertial force generated by a desired motion of the robot 1 and the gravity acting on the robot 1 becomes zero. Incidentally, “the body position” is the position of a certain representative point of the body 24.

A desired total floor reaction force determined by the gait generating device 100 is constructed of the desired values of the translational force and the moment acting on a desired total floor reaction force central point, and it is determined such that it balances with the resultant force of the inertial force generated by a desired motion of the robot 1 and the gravity acting on the robot 1 at a desired total floor reaction force central point in the present embodiment. The moment horizontal component of the desired total floor reaction force about the desired total floor reaction force central point (desired ZMP) is zero. Supplementally, it is not necessary to determine all components of the translational force and the moment acting on the desired total floor reaction force central point as desired values. For instance, if the posture or the floor reaction force of the robot 1 about the vertical axis is not controlled, then it is not necessary to determine the component of the moment of the desired total floor reaction force about the vertical axis (the component in the Z-axis direction).

The desired ground contact portion trajectory determined by the gait generating device 100 (the desired ground contact portion position trajectory) is corrected by a hierarchical compliance operation determiner 114, which will be discussed later.

The desired floor reaction force distributor 102 groups (that is, hierarchizes) the first to the fourth ground contact portions 10 of the robot 1 into a tree structure, and associates the nodes of the tree structure with each of the hierarchized groups. Hence, in the following explanation, the nodes may be expressed by replacing them by the groups. Each node is a group constructed of one or more ground contact portions 10.

In the first embodiment, the ground contact portions 10 are hierarchized, as shown in FIG. 6. More specifically, the n-th ground contact portion 10 (n=1,2,3,4) is associated with the n-th node, the node having a first node and a fourth node as child nodes is defined as a 14th node, the node having a second node and a third node as child nodes is defined as a 23rd node, and the node having the 14th node and the 23rd node as child nodes is defined as the 1423rd node. Thus, the first to the fourth nodes are nodes that are constructed of the first, the second, the third, and the fourth ground contact portions 10, respectively, the 14th node is a node constructed of the first ground contact portion 10 and the fourth ground contact portion 10, the 23rd node is a node constructed of the second ground contact portion 10 and the third ground contact portion 10, and the 1423rd node is the node constructed of all the ground contact portions 10.

According to general designations in a tree structure, a node having no child node is referred to as a leaf node, and a node having no parent node is referred to as a root node. Thus, the n-th node (n=1,2,3,4) is a leaf node, while the 1423rd node is a root node. In the present description, to identify nodes, leaf nodes will be assigned the same numbers (1,2,3, . . . ) as those of the ground contact portions (or legs) associated therewith, while nodes other than the leaf nodes will be assigned with numbers that are greater than those of the leaf nodes. Further, the nodes other than the leaf nodes and the root node will be referred to as intermediate nodes. In the first embodiment, the intermediate nodes are the 14th node and the 23rd node.

Input to the desired floor reaction force distributor 102 are a desired total floor reaction force central point trajectory, a desired total floor reaction force trajectory and a desired ground contact portion trajectory out of a desired gait determined by the gait generating device 100. The gait parameters (the estimated landing position, an estimated landing time, etc. of the ground contact portion 10 of a free leg of the robot 1) used by the gait generating device 100 to determine the desired gait may be also input to the desired floor reaction force distributor 102. Then, on the basis of these inputs, the desired floor reaction force distributor 102 determines desired node floor reaction force central points (desired n-th node floor reaction force central points) Qn (n=1,2,3,4,14,23,1423), which are the desired positions of the floor reaction force central points of the nodes hierarchized as described above, and weights Wn (n=1,2,3,4,14,23) of the nodes except the root node. Each weight Wn will be a nonnegative value of 1 or less.

The technique for determining desired node floor reaction force central points Qn (n=1,2,3,4,14,23) and the weights Wn (n=1,2,3,4,14,23) will be explained in detail below with reference mainly to FIG. 3(b), FIG. 5 and FIG. 7. FIG. 3(b) shows a relationship between Qn and Wn in a state illustrated in the figure, FIGS. 5(a) to (c) show setting examples of Wn, and FIG. 7 shows a relationship between weights and desired node floor reaction force central points.

The desired node floor reaction force central point of the 1423rd node, which is the root node, (the desired 1423rd node floor reaction force central point) Q1423 is determined to be identical to the desired total floor reaction force central point (point P in FIG. 3 and FIG. 4) (P=Q1423). Accordingly, in the following explanation, the same reference character P as that of a desired total floor reaction force central point will be frequently used as the mark denoting the desired 1423rd node floor reaction force central point.

Further, a weight W1423 of the root node is set to “1” for the sake of convenience in order to maintain the uniformity of expressions.

In the present embodiment, the desired floor reaction force central point of an n-th ground contact portion 10 (n=1,2,3,4) (also referred to as a desired n-th ground contact portion floor reaction force central point) is always set to the central point of an n-th ground contact portion 10 (the central point of the spherical joint 12). Further, the desired node floor reaction force central point Qn (n=1,2,3,4) of a leaf node associated with the n-th ground contact portion 10 is determined to be identical to the desired floor reaction force central point of the n-th ground contact portion 10. Hence, the desired node floor reaction force central point Qn (n=1,2,3,4) is also always set to the central point of the n-th ground contact portion 10 (the position of this point is uniquely determined from a desired ground contact portion position of the n-th ground contact portion 10). The position of the desired node floor reaction force central point Qn (n=1,2,3,4) of the leaf node determined as described above is uniquely determined from the desired ground contact portion position of the n-th ground contact portion 10. In other words, the desired node floor reaction force central point Qn (n=1,2,3,4) of a leaf node defines the desired position of the n-th ground contact portion 10. In the following explanation, the desired node floor reaction force central point of a leaf node may be referred to as a desired ground contact portion floor reaction force central point in some cases. Each desired ground contact portion trajectory is set such that it continuously changes, so that Qn (n=1,2,3,4) will also continuously change.

Supplementally, in the present embodiment, the desired floor reaction force central point Qn (n=1,2,3,4) of an n-th leaf node (an n-th ground contact portion 10) has been set to the central point of the n-th ground contact portion 10; however, in a robot that allows the postures of ground contact portions to be controlled, the desired floor reaction force central point of each leaf node (each ground contact portion) may be set, for example, within the ground contact surface of the ground contact portion defined by the desired position/posture of the ground contact portion associated with the leaf node (the surface that comes in contact with a floor surface assumed in a desired gait).

Hereinafter, in general, a segment that connects arbitrary two points A and B or the length thereof will be denoted as AB. In addition, an operator “*” means multiplication for a pair of scalar and scalar or a pair of scalar and vector, while it means an outer product (vector product) for a pair of vector and vector.

The weight Wn (n=1,2,3,4,14,23) of each node except the root node and desired node floor reaction force central points Q14 and Q23 of the intermediate nodes are determined such that a desired 14th node floor reaction force central point Q14 will be the internally dividing point of a segment Q1Q4 that satisfies a relational expression, Q1Q14:Q14Q4=W4:W1=(1−W1):W1, a desired 23rd node floor reaction force central point Q23 will be the internally dividing point of a segment Q2Q3 that satisfies a relational expression, Q2Q23:Q23Q3=W3:W2=(1−W2):W2, and a desired 1423rd node floor reaction force central point P (=desired total floor reaction force central point) will be the internally dividing point of a segment Q14Q23 that satisfies Q14P:PQ23=W23:W14=(1−W14):W14.

In other words, Q14, Q23 and Wn (n=1,2,3,4,14,23) are determined to satisfy at least the following expressions 1, 2 and 3. Incidentally, Qn (n=1,2,3,4,14,23) in these expressions 1 to 3 means the position (positional vector) of the point. Q14=Q1*W1+Q4*W4, W1+W4=1   Expression 1 Q23=Q2*W2+Q3*W3, W2+W3=1   Expression 2 P=Q14*W14+Q23*W23, W14+W23=1   Expression 3

Wn (n=1,2,3,4,14,23) takes a nonnegative value of 1 or less, so that the coefficients (weights) of Qn (n=1,2,3,4,14,23) in the right sides of the above expressions 1, 2 and 3 will be all nonnegative values.

The above expressions 1 to 3 mean that the position of a desired node floor reaction force central point of each node having child nodes (that is, each node that is not a leaf node) is set to a weighted average position of the position of the desired node floor reaction force central points of the child nodes of the node by using a predetermined nonnegative weight. In other words, as shown in FIG. 3(b) and FIG. 7 mentioned above, the desired floor node reaction force central point Qn (n=14,23,1423) of each node having child nodes is set at the internally dividing point of the desired node floor reaction force central points of all the child nodes of the node. FIG. 7 is a diagram showing a relationship between the desired node floor reaction force central points Qn (n=1,2,3,4,14,23,1423) of nodes and the weight Wn (n=1,2,3,4,14,23). Incidentally, L23, L14 and L1423 in FIG. 3(b) denote segments Q2Q3, Q1Q4 and Q23Q14, respectively.

Supplementally, Q1 to Q4 and P (=Q1423) are determined as described above, so that once Wn (n=1,2,3,4,14,23) is determined, Q14 and Q23 that satisfy expressions 1 to 3 are uniquely determined. In other words, if Q14 and Q23 are determined, then Wn (n=1,2,3,4,14,23) satisfying expressions 1 to 3 will be uniquely determined. Accordingly, the weight Wn (n=1,2,3,4,14,23) may be determined and then the desired node floor reaction force central points Q14 and Q23 of intermediate nodes according to the above expressions 1 to 3 may be determined, or the desired node floor reaction force central points Q14 and Q23 of the intermediate node may be determined and then the weight Wn (n=1,2,3,4,14,23) may be determined according to the above expression 1 to 3. Either way may be used.

The desired node floor reaction force central points Q14 and Q23 of the intermediate nodes move as the ground contact portions 10 move, as shown in time series in FIG. 3(a) to FIG. 3(c) and FIG. 4(a) to FIG. 4(c). Weights W14 (=1−W23), W1 (=1−W4) and W3 (=1−W2) at this time are determined such that they continuously change, as shown in, for example, the graphs of FIGS. 5(a) to (c), respectively. The trajectory of Qn and the weight Wn of the n-th node (n=1,2,3,4) are set such that they continuously change, so that the desired node floor reaction force central points Q14 and Q23 are also set such that they continuously move. This means that all desired node floor reaction force central points (desired n-th node floor reaction force central points (n=1,2,3,4,14,23,1423)) are set such that they continuously move.

The desired floor reaction force distributor 102 outputs the desired node floor reaction force central point of each node determined as described above. The desired floor reaction force central point of the root node is the same as the desired total floor reaction force central point determined by the gait generating device 100, so that it does not have to be output from the desired floor reaction force distributor 102.

The desired floor reaction force distributor 102 determines and outputs a desired node floor reaction force, which is the desired value of the floor reaction force acting on the desired floor reaction force central point of each node. The desired node floor reaction force that is output includes at least a desired node floor reaction force (a desired n-th node floor reaction force) acting on a desired node floor reaction force central point Qn of an n-th node (n=1,2,3,4), which is a leaf node, that is, the desired floor reaction force of each ground contact portion 10. This desired node floor reaction force is necessary primarily to compensate for the flexures of the compliance mechanisms 42 or the like (refer to FIG. 1) of the legs #1 to #4 (more specifically, compensation for the positional displacement of the ground contact portions 10 caused by the flexures of the compliance mechanisms 42 and the link mechanisms of the legs), the compensation being carried out by the processing of a hierarchical compliance operation determiner 114, which will be discussed later. Supplementally, if the robot 1 is provided with compliance mechanisms in addition to those on the distal portions of the legs #1 to #4, then preferably, a desired 14th node floor reaction force and a desired 23rd node floor reaction force (the desired node floor reaction forces of intermediate nodes) are also determined and output in order to determine the deformations of the compliance mechanisms.

Generally, a desired floor reaction force (a desired node floor reaction force) acting on a desired node floor reaction force central point of each node may be determined from a desired total floor reaction force and the weight of each node. Specifically, the desired floor reaction force of any one node may be determined by multiplying the product of the weight of the node and the weight of all ancestor nodes of the node by a desired total floor reaction force. More specifically, a desired n-th node floor reaction force is calculated according to the following expression 4a (or expression 4b). $\begin{matrix} {{{Desired}\quad n\text{-}{th}\quad{node}\quad{floor}\quad{reaction}\quad{force}} = {{Weight}\quad{of}\quad{the}\quad n\text{-}{th}\quad{node}*{Product}\quad{of}\quad{the}\quad{weights}\quad{of}\quad{all}\quad{ancestor}\quad{nodes}\quad{of}\quad{the}\quad n\text{-}{th}\quad{node}*{Desired}\quad{total}\quad{floor}\quad{reaction}\quad{force}}} & {{Expression}\quad 4a} \\ {{{{Desired}\quad n\text{-}{th}\quad{node}\quad{floor}\quad{reaction}\quad{force}} = {{Weight}\quad{of}\quad{the}\quad n\text{-}{th}\quad{node}*{Product}\quad{of}\quad{the}\quad{weight}\quad{of}\quad{all}\quad{ancestor}\quad{nodes}\quad\left( {{excluding}\quad{the}\quad{root}\quad{node}} \right)\quad{of}\quad{the}\quad n\text{-}{th}\quad{node}*{Desired}\quad{total}\quad{floor}\quad{reaction}\quad{force}}}\quad} & {{Expression}\quad 4b} \end{matrix}$

Alternatively, desired node floor reaction forces are determined such that the desired floor reaction force of an arbitrary n-th node that is not a leaf node agrees with the sum (resultant force) of the desired floor reaction forces of all child nodes of the n-th node and the desired floor reaction force of the root node agrees with a desired total floor reaction force. This relationship is shown in FIG. 8.

More specifically, if a desired n-th node floor reaction force, which is the desired floor reaction force of an n-th node, is denoted by Fn (n=1,2,3,4,14,23,1423) and a desired total floor reaction force is denoted by Ftotalref, then Fn is determined from Ftotalref and the weight Wn (n=1,2,3,4,14,23) according to the expressions shown in FIG. 8. The expressions in FIG. 8 are equivalent to the above expression 4a or 4b.

Determining the desired floor reaction force central point of each node (a desired node floor reaction force central point) and the desired floor reaction force of each node (a desired node floor reaction force) as described above is to determine the desired floor reaction force central point and the desired floor reaction force of each node such that the horizontal component of the moment generated about a desired n-th node floor reaction force central point by the resultant force of the desired floor reaction forces of all child nodes of an n-th node becomes zero. Accordingly, the moment horizontal component of a desired node floor reaction force is zero for any nodes.

A set of each desired node floor reaction force central point Qn (n=1,2,3,4,14,23), each weight Wn(n=1,2,3,4,14,23) and each desired node floor reaction force Fn (n=1,2,3,4,14,23) is determined such that the desired floor reaction force of the ground contact portion 10 not in contact with the ground (in a no-contact-with-ground state)(the desired node floor reaction force of a leaf node associated with the ground contact portion 10 not in contact with the ground) is zero. Hence, in the first embodiment, the weight W14 is set to zero (W23=1) during a period in which the first ground contact portion 10 and the fourth ground contact portion 10 are free legs and off a floor (the ground contact portions 10 of the free legs are moving in the air), while the weight W23 is set to zero (W14=1) during a period in which the second ground contact portion 10 and the third ground contact portion 10 are free legs and off a floor, as shown in FIG. 5(a). Thus, the weight of an intermediate node having child nodes is set to zero during a period in which none of the ground contact portions belonging as leaf nodes to the intermediate node are in contact with the ground. In other words, the weight of an intermediate node is not set to zero during a period in which any one of the ground contact portions belonging to the intermediate node is in contact with the ground (strictly speaking, a period during which a non-zero floor reaction force is acting on any one of the ground contact portions).

Supplementally, the first embodiment does not have a period during which only one of the first ground contact portion 10 and the fourth ground contact portion 10 belonging to the intermediate node Q14 is placed in a no-contact-with-ground state; therefore, the weights W1 and W4 corresponding to these ground contact portions 10 and 10 do not have any periods during which they are 0 or 1. If, however, a desired ground contact portion trajectory is determined so as to include a period during which only one of the first ground contact portion 10 and the fourth ground contact portion 10 will be placed in the no-contact-with-ground state, then the weight associated with the ground contact portion 10 that will be placed in the no-contact-with-ground state during the period may be set to 0, and the weight associated with the ground contact portion 10 to be in contact with the ground may be set to 1. In this case, the weight of the intermediate node Q14 during that period will be set to a non-zero value. The same applies to the weights associated with the intermediate node Q23 and the second ground contact portion 10 and the third ground contact portion 10, which are leaf nodes belonging thereto. Generally speaking, the weight of a node having child nodes is set to a non-zero value if any one of the ground contact portions belonging to the node is in contact with the ground, while it is set to zero if all of the ground contact portions belonging to the node are in no-contact-with the ground.

The desired node floor reaction forces determined as described above continuously change, making them suited for achieving a movement (walking) of the robot 1 with less impact.

As described above, according to the first embodiment, each desired node floor reaction force central point Qn (n=1,2,3,4,14,23,1423), each weight Wn(n=1,2,3,4,14,23) and each desired each floor reaction force Fn (n=1,2,3,4,14,23,1423) are determined by the desired floor reaction force distributor 102 such that they satisfy the following conditions A) to G).

-   A) The desired node floor reaction force central point Qn(n=1,2,3,4)     of each leaf node agrees with the central point of the ground     contact portion 10 corresponding to the leaf node. More generally,     Qn(n=1,2,3,4) is determined according to a desired gait (a desired     motion, such as a desired ground contact portion trajectory).     Supplementally, for example, the desired floor reaction force     central point of each ground contact portion 10 may be determined by     the gait generating device 100, and in this case, the desired node     floor reaction force central point Qn(n=1,2,3,4) may be determined     on the basis of the desired floor reaction force determined by the     gait generating device 100. -   B) The desired node floor reaction force central point of the root     node agrees with a desired total floor reaction force central point     P. -   C) The desired node floor reaction force central point     Qn(n=1,2,3,4,14,23) and the weight Wn (n=1,2,3,4,14,23) of each node     excluding the root node satisfy the relational expressions of the     above expressions 1 to 3. In other words, the desired node floor     reaction force central point Qn of an arbitrary n-th node     (n=14,23,1423) having child nodes will be a weighted average point     of the desired node floor reaction force central points of the child     nodes of the n-th node. -   D) The desired node floor reaction force Fn of an arbitrary n-th     node (n=14,23,1423) having child nodes agrees with the sum     (resultant force) of the desired node floor reaction forces of all     child nodes of the n-th node, and the desired node floor reaction     force F1423 of the root node (the 1423rd node) agrees with the     desired total floor reaction force Ftotalref. In other words, the     desired floor reaction force Fn and the weight Wn of the n-th node     (n=1,2,3,4,14,23,1423) satisfy the relational expressions in FIG. 8. -   E) The desired node floor reaction force of a leaf node associated     with the ground contact portion 10 not in contact with the ground is     zero. -   F) Qn, Wn, and Fn (n=1,2,3,4,14,23,1423) continuously change. -   G) The weight of a leaf node corresponding to the ground contact     portion 10 in a no-contact-with-ground state or the weight of any     one of the ancestor nodes of the leaf node is zero.

Incidentally, a desired node floor reaction force may be determined on the basis of each desired node floor reaction force central point in place of determining it on the basis of a weight as described above. More specifically, each desired node floor reaction force central point may be determined such that the aforesaid conditions A) to C) are satisfied, then each weight may be determined on the basis of the desired node floor reaction force central point and the aforesaid expressions 1 to 3, and then the desired node floor reaction force may be determined according to the aforesaid expression 4 by using the determined weight.

Returning to the explanation of FIG. 2, the posture error calculator 103 calculates the error of an actual body posture with respect to a desired body posture and outputs the calculated error to the robot 1. In the present embodiment, the posture error calculator 103 receives an inclination angle of the body 24 relative to the vertical direction detected by the posture sensor 36 (hereinafter referred to as the actual body posture inclination) and a desired body position/posture determined by the gait generating device 100 (specifically, the inclination angle in a desired body posture relative to the vertical direction; hereinafter referred to as the desired body posture inclination), and then calculates an error θberr between them (Actual body posture inclination−Desired body posture inclination; hereinafter referred to as the body posture inclination error θberr). The calculated θberr is composed of a component about the X axis (the component in the roll direction) θberrx and a component about the Y axis (the component in the pitch direction) θberry. If a desired body posture inclination is, for example, steadily zero, then θberr=actual body posture inclination, so that a detected value (actual body posture inclination) of a posture sensor 36 may be directly output as the body posture inclination error θberr.

Generally speaking, the posture stabilization control calculator 104 calculates a compensating total floor reaction force, which is a compensation amount of a total floor reaction force (the correction amount of a desired total floor reaction force) for stabilizing the posture of the robot 1 according to the state of the robot 1 that is detected or estimated on the basis of the information of a sensor provided in the robot 1, such as the aforesaid body posture inclination error.

To stabilize the posture of the robot 1 in the long term, a translational force and a moment necessary to restore the actual position/posture of a predetermined portion, such as the body 24, of the robot 1 to a desired position/posture are determined, and these have to be additionally generated at the desired total floor reaction force central point (desired ZMP) being the point of action. The additional translational force and moment are referred to as a compensating total floor reaction force. The moment component of a compensating total floor reaction force is referred to as a “compensating total floor reaction force moment Mdmd (specifically, a compensating total floor reaction force moment Mdmd about a desired total floor reaction force central point (desired ZMP)).

In the present embodiment, the posture stabilization control calculator 104 calculates the compensating total floor reaction force moment Mdmd so as to restore (approximate) an actual body posture inclination to a desired body posture inclination. Hence, the body posture inclination errors θberr (θberrx, θberry) determined by the posture error calculator 103 are input to the posture stabilization control calculator 104.

The posture stabilization control calculator 104 calculates the compensating total floor reaction force moment Mdmd on the basis of the input body posture inclination error θberr. The calculated Mdmd is composed of a component about the X axis Mdmdx and a component about the Y axis Mdmdy.

Specifically, Mdmdx and Mdmdy are determined by, for example, the feedback control law (PD control law here) of expressions 5 and 6 given below. More specifically, Mdmdx and Mdmdy are determined so as to approximate the body posture inclination errors θberrx and θberry to zero. Mdmdx=−Kthx*θberrx−Kwx*(dθberrx/dt)   Expression 5 Mdmdy=−Kthy*θberry−Kwy*(dθberry/dt)   Expression 6 where Kthx, Kthy, Kwx and Kwy denote predetermined gains. Further, (dθberrx/dt) and (dθberry/dt) denote the time differential values of the body posture inclination errors θberrx and θberry, respectively.

In the present embodiment, a component of the compensating total floor reaction force moment Mdmd about the Z axis (the component in the yaw direction) Mdmdz is not used, so that Mdmdz is not determined; however, Mdmdz may be determined to prevent the robot 1 from spinning (slippage about the vertical axis). A method for determining Mdmdz is explained in detail in Japanese Patent Application No. 2003-185613 or Japanese Patent Application No. 2003-185930 previously proposed by the present applicant. Moreover, for example, to restore the position of an actual center-of-gravity of the robot 1 to the position of the center-of-gravity in a desired gait, the translational force of a compensating total floor reaction force can be determined on the basis of a positional error of the center-of-gravity.

The floor reaction force detector 108 detects the actual floor reaction forces, which are the actual values of the floor reaction forces acting on the ground contact portions 10 of the actual robot 1 (that is, the actual floor reaction forces of leaf nodes (actual node floor reaction forces)) on the basis of the outputs of the six-axis force sensor 34 of the legs #1 to #4. In addition, the floor reaction force detector 108 calculates the relative positions/postures (the relative positions in the first embodiment) of the ground contact portions 10 relative to the coordinate system fixed to the body 24 on the basis of the actual joint displacements of the joints 14 and 15 (the actual rotational angles of the joints 14 and 15 about individual rotational axes) of the legs #1 to #4 detected by sensors, such as encoders, (not shown) provided on the joints 14 and 15 of the robot 1. At this time, joint displacement commands, which are the displacement command values (rotational angle command values) of the joints 14 and 15 may be used in place of actual joint displacements, or both actual joint displacements and joint displacement commands may be used. Then, based on the calculated relative positions/postures of the ground contact portions 10, the detected values of the six-axis force sensor 34 of the legs #1 to #4 (these being the values on a local coordinate system fixed to the six-axis force sensor 34 or the like) are coordinate-converted to calculate actual floor reaction forces represented on the coordinate system fixed to the body 24, and then the calculated actual floor reaction forces are converted into the actual floor reaction forces represented on a supporting leg coordinate system (global coordinate system). For the coordinate conversion into the supporting leg coordinate system, detected values of the posture sensor 36 or desired body posture inclinations may be used. Supplementally, in the robot 1 according to the first embodiment, no floor reaction force moment acts on the central points of the ground contact portions 10, as described above, so that there is no need to detect a moment component in the actual floor reaction force of each ground contact portion 10. In this case, as mentioned above, in place of the six-axis force sensor 34, a three-axis force sensor may be used to detect translational force components of actual floor reaction forces in three axes, or a one-axis floor reaction force sensor may be used to detect only translational force vertical components of actual floor reaction forces.

Based on a final desired trajectory of each ground contact portion position/posture (this being determined by a hierarchical compliance operation determiner 114, which will be discussed later) and desired body position/posture or the like, the robot geometric model (inverse kinematics calculator) 110 performs inverse kinematics calculation to calculate joint displacement commands, which are the command values of the displacements (rotational angles) of the joints 14 and 15 of the robot 1, that satisfy the above. In the present embodiment, the equation of the solution of the inverse kinematics calculation has been determined beforehand, and the joint displacement commands have been calculated simply by substituting desired body position/posture and the final desired position of each ground contact portion into the equation. More specifically, the robot geometric model 110 receives a desired body position/posture trajectory determined by the gait generating device 100 and a corrected desired ground contact portion trajectory corrected as will be discussed later by the hierarchical compliance operation determiner 114 (a corrected desired ground contact portion trajectory with deformation compensation), and calculates the joint displacement commands of the joints 14 and 15 of the legs #1 to #4 by the inverse kinematics calculation from the received values.

In the case of a robot having joints, such as arm joints and a neck joint, in addition to leg joints, as in the sixth embodiment to be discussed later, the displacements of joints other than the leg joints are determined by the inverse kinematics calculation on the basis of the relative positions/postures of hands, the head or the like with respect to the body.

The displacement controller 112 receives the actual joint displacements (detected values) of the joints 14 and 15 of the robot 1 and the joint displacement commands calculated by the robot geometric model (inverse kinematics calculator) 110, and controls (feedback-control) actuators (not shown) of the joints 14 and 15 by using the joint displacement commands as the desired values such that the actual joint displacements follow the desired values.

The hierarchical compliance operation determiner 114 corrects a desired ground contact portion trajectory to approximate an actual total floor reaction force to the resultant force of a desired total floor reaction force and a compensating total floor reaction force, and outputs a corrected desired ground contact portion position/posture trajectory with deformation compensation, which is the desired ground contact portion trajectory after the correction. In the present embodiment, the postures of the ground contact portions 10 cannot be controlled; therefore, the corrected desired ground contact portion position/posture trajectory with deformation compensation is actually a corrected desired ground contact portion position trajectory with deformation compensation.

The hierarchical compliance operation determiner 114 generally corrects the desired ground contact portion trajectories of the ground contact portions 10 so as to satisfy the following three requirements as much as possible.

-   Requirement 1) In order to stabilize the position/posture of the     robot 1, an actual total floor reaction force is made to follow the     resultant force of a compensating total floor reaction force (moment     Mdmd) output by the posture stabilization control calculator 104 and     a desired total floor reaction force. According to the first     embodiment, in order to stabilize the posture inclination (the     inclination relative to the vertical direction) of the body 24 of     the robot 1, the horizontal component of an actual total floor     reaction force moment about a desired total floor reaction force     central point is made to follow horizontal components Mdmdx and     Mdmdy of the compensating total floor reaction force moment Mdmd.     Supplementally, the horizontal component of the desired total floor     reaction force moment about the desired total floor reaction force     central point is zero, so that the resultant force of this and Mdmd     agrees with Mdmd. -   Requirement 2) The absolute value of the actual floor reaction force     moment about a desired floor reaction force central point of each     node, which is not a leaf node, is minimized as much as possible to     prevent an actual floor reaction force from being focused on some     ground contact portions 10 of a plurality of ground contact portions     10 to be in contact with the ground, causing the actual floor     reaction force on other ground contact portions 10 to be excessively     reduced, which will lead to extremely deteriorated ground contacting     properties of the ground contact portions 10 having the reduced     actual floor reaction force. In the first embodiment, the absolute     values of the actual floor reaction force moments about the desired     floor reaction force central points of the 14th node, the 23rd node     and the 1423rd node are minimized as much as possible. -   Requirement 3) In order to secure the ground contacting properties     of the ground contact portions 10 to be in contact with the ground,     that is, to prevent local ground contacting properties of the ground     contact portions 10 from deteriorating due to uneven local ground     contact pressure distribution (the distribution of an actual floor     reaction force) in the ground contact portions 10, the absolute     values of the actual floor reaction force moments about the desired     floor reaction force central points of the ground contact portions     10 (leaf nodes) are minimized as much as possible. However, in the     robot 1 of the first embodiment, the actual floor reaction force     moments about the desired floor reaction force central points of the     ground contact portions 10 are always zero; therefore, it is     unnecessary to consider this requirement 3).

In general, it is impossible to fully satisfy all of requirements 1) to 3) or requirements 1) and 2). For example, it is often physically impossible to zero the actual floor reaction force moments about the desired floor reaction force central points of the ground contact portions 10 while making an actual total floor reaction force agree with the resultant force of a compensating total floor reaction force and a desired total floor reaction force. Hence, the hierarchical compliance operation determiner 114 usually corrects the desired ground contact portion trajectories of the ground contact portions 10 at certain compromise points while satisfying requirements 1) to 3) or requirements 1) and 2) as much as possible.

The above has given the overviews of the functional means (functional components) of the control device 50.

Supplementally, the hierarchical compliance operation determiner 114, the posture stabilization control calculator 104 and a robot geometric model (inverse kinematics calculator) 110 correspond to the node operation control means in the present invention.

Referring now to the flowchart of FIG. 9, the overall operation (arithmetic processing) of the control device 50 will be explained in more detail. FIG. 9 is a flowchart (structured flowchart) showing the main routine processing of the control device 50. The components of the control device 50 carrying out pertinent processing are shown on the left in FIG. 9.

First, the initialization of the control device 50 is performed in S10, then the processing advances to S14 via S12. The arithmetic processing of the control device 50 waits for a timer interrupt for each control cycle. The control cycle is, for example, 50 ms.

Subsequently, the processing proceeds to S16 wherein it determines whether a gait change is observed, and if the determination result is NO, then it proceeds to S22, which will be discussed later. If the determination result in S16 is YES, then the processing proceeds to S18 wherein it initializes time t to zero, and proceeds to S20 wherein it sets gait parameters. In the present embodiment, for example, a desired gait of a predetermined period from the moment a predetermined leg (e.g., #1) of the robot 1 leaves a floor to the moment it leaves the floor next (or from the moment it lands to the moment it lands next) is taken as a unit, and gait parameters that are the parameters defining the desired gait for the predetermined period (the parameters used in the algorithm for determining the desired gait) are set in S20. The “gait change” mentioned in S16 means the change of the desired gait for the predetermined period. Whether the desired gait is changing may be determined mainly on the basis of time or a detected value of the six-axis force sensor 34 on the predetermined leg.

The gait parameters set in S20 are composed of motion parameters that define the desired motion trajectories (specifically, a desired ground contact portion trajectory and a desired body position/posture trajectory) of the robot 1 and the floor reaction force parameters that define desired floor reaction force trajectories (specifically, a desired total floor reaction force trajectory and a desired total floor reaction force central point trajectory). Supplementally, once the desired motion of the robot 1 is determined, the desired total floor reaction force can be obtained by reversing the sign of the resultant force of the inertial force generated by the desired motion and the gravity acting on the robot 1, so that the floor reaction force parameters may be the ones that define only a desired total floor reaction force central point trajectory. Further, motion parameters do not have to include parameters that define a desired body position/posture trajectory if a desired ground contact portion trajectory (more generally, the parameter of a desired motion other than desired body position/posture) and a desired total floor reaction force central point are determined, and then desired body position/posture are determined using a dynamic model of the robot 1 such that the horizontal component of a moment generated about a desired total floor reaction force central point (desired ZMP) by the resultant force of the inertial force generated by a desired motion, including desired body position/posture of the robot 1, and the gravity acting on the robot 1 becomes zero.

Subsequently, the processing proceeds to S22 wherein the instantaneous value of a desired gait is determined on the basis of the aforesaid gait parameters. Here, “instantaneous value” means a value for each control cycle, and a desired gait instantaneous value is composed of the instantaneous values of desired body position/posture, a desired ground contact portion position (the instantaneous value of a desired ground contact portion trajectory), a desired total floor reaction force, and a desired total floor reaction force central point position (a desired ZMP position). In the present embodiment, the postures of the ground contact portions 10 cannot be controlled, so that the instantaneous values of the desired postures of the ground contact portions 10 are not determined. In a case where the postures of the ground contact portions can be controlled, the parameters defining the desired postures of the ground contact portions may be included in the gait parameters and the instantaneous values of the desired postures of the ground contact portions may be determined on the basis of the parameters.

The processing of S14 to S22 described above is the processing carried out by the gait generating device 100.

Subsequently, the processing proceeds to S24 wherein the weight Wn(n=1,2,3,4,14,23) of each node and a desired floor reaction force central point of each node (a desired node floor reaction force central point) Qn(n=1,2,3,4,14,23) are determined. This processing is the processing carried out as described above by the desired floor reaction force distributor 102.

Subsequently, the processing proceeds to S26 wherein desired node floor reaction forces (including at least the desired floor reaction forces of the ground contact portions 10 (leaf nodes)) are determined. In the first embodiment, the desired node floor reaction forces of leaf nodes (desired ground contact portion floor reaction forces) are determined. This processing of S26 is also the processing carried out as described above by the desired floor reaction force distributor 102. As described above, if the compliance mechanisms are provided on other portions in addition to the distal portions of the legs #1 to #4, then the desired floor reaction forces of intermediate nodes that are not leaf nodes should be also determined. Supplementally, the moment horizontal component of a desired node floor reaction force is zero.

Subsequently, the processing proceeds to S28 wherein the state of the robot 1, such as the actual body posture inclination, is detected from an output of the posture sensor 36 or the like. In the first embodiment, the value of an actual body posture inclination detected by the posture sensor 36 is captured by the posture error calculator 103, and a body posture inclination error θberr is calculated from the detected value and the desired body posture inclination (the instantaneous value at the current time) out of the desired body position/posture.

Subsequently, the processing proceeds to S30 wherein a compensating total floor reaction force for stabilizing the posture of the robot 1 is determined from the state of the robot 1 detected in S28. In the first embodiment, the horizontal components Mdmdx and Mdmdy of a compensating total floor reaction force moment Mdmd about a desired total floor reaction force central point (desired ZMP) are calculated by the posture stabilization control calculator 104 from the body posture inclination error θberr according to the above expression 5 and expression 6.

Subsequently, the processing proceeds to S32 wherein the actual floor reaction force of each ground contact portion 10 is detected. This is the processing carried out by the actual floor reaction force detector 108. As described above, the actual floor reaction force for each ground contact portion 10 detected by the six-axis force sensor 34 that is converted to a supporting leg coordinate system (global coordinate system) is determined. Hereinafter, the actual floor reaction force of each ground contact portion 10 may be referred to as an actual ground contact portion floor reaction force in some cases.

Subsequently, from S34 to S38, the processing of the hierarchical compliance operation determiner 114 is carried out.

For a while, the following will explain the overall processing of the hierarchical compliance operation determiner 114 before the processing of these S34 to S38 is specifically explained. In this explanation, for the convenience of understanding, the state shown in FIG. 3(b) mentioned above (the state wherein all the legs #1 to #4 of the robot 1 are supporting legs) will be primarily taken as an example.

The hierarchical compliance operation determiner 114 determines the translational force components and moment components of the desired node floor reaction forces of nodes excluding leaf nodes (more specifically, the translational force components and the moment components that have desired node floor reaction force central points as the points of action) mainly on the basis of the desired node floor reaction forces of the leaf nodes determined by the desired floor reaction force distributor 102.

The translational force components of the desired floor reaction forces (desired node floor reaction forces) of the nodes in the state shown in FIG. 3(b) are illustratively shown in FIG. 10. In this figure, a vector Fn_ref (n=1,2,3,4,14,23) denotes the translational force component of a desired n-th node floor reaction force. Further, Ftotalref denotes the translational force component of a desired total floor reaction force (=desired 1423rd node floor reaction force). As shown in FIG. 8 mentioned above, a desired n-th node floor reaction force of an arbitrary n-th node (n=14,23,1423), which is not a leaf node, is determined by the resultant force of the desired node floor reaction forces of all child nodes of the n-th node. Therefore, F14ref=F1ref+F4ref, F23ref=F2ref+F3ref, and Ftotalref(=F1423ref)=F14ref+F23ref. The hierarchical compliance operation determiner 114 determines the translational force components F14ref(=Flref+F4ref) and F23ref (=F2ref+F3ref) of the desired node floor reaction forces of intermediate nodes, as described above, from the translational force components Fn_ref(n=1,2,3,4) of the desired node floor reaction forces of the leaf nodes (the ground contact portion 10) determined by the desired floor reaction force distributor 102. Ftotalref (=F1423ref) is set to the translational force component of the desired total floor reaction force determined by the gait generating device 100.

Furthermore, the hierarchical compliance operation determiner 114 determines the moment component of a desired node floor reaction force of each node, excluding leaf nodes, having a desired node floor reaction force central point being the point of action, as in the case of the translational force component of a desired node floor reaction force. In this case, however, according to the definition of a desired node floor reaction force central point Qn (n=1,2,3,4,14,23), the moment horizontal component of a desired n-th node floor reaction force is always set to zero. The moment horizontal component of a desired 1423rd node floor reaction force (=desired total floor reaction force) is also set to zero.

In the robot 1 of the present embodiment, the ground contact portions 10 are engaged with the spherical joints 12 (free joints) at the distal end portions of the legs #1 to #4, so that floor reaction force moments (horizontal components and vertical components) cannot be generated in the ground contact portions 10 (leaf nodes). For this reason, in the hierarchical compliance operation determiner 114, the moment vertical components of the desired node floor reaction forces of the ground contact portions 10 (leaf nodes) are also set to zero.

If a desired node floor reaction force moment vertical component of a node that is not a leaf node is determined so as to dynamically balance with a desired motion of the robot 1, then it could generally take a non-zero value; however, in the present embodiment, the control related to the rotation (the rotation in the yaw direction) of the posture of the robot 1 about the vertical axis is not carried out. In the present embodiment, therefore, the setting of the vertical component of the moment of a desired node floor reaction force of a node that is not a leaf node will be omitted. For this reason, the moment components of desired node floor reaction forces in the state shown in FIG. 3(b) will not be shown. If the control related to the rotation of the posture of the robot 1 about the vertical axis is carried out, then the desired floor reaction force moment vertical components of the nodes should be also set.

Supplementally, if the translational force components and the moment components of the desired node floor reaction forces of the nodes, including leaf nodes are determined by the desired floor reaction force distributor 102, then the determination need not be performed by the hierarchical compliance operation determiner 114.

Further, the hierarchical compliance operation determiner 114 also determines the translational force component and the moment component of an actual node floor reaction force, which is the actual floor reaction force of each node.

The translational force components of the actual floor reaction forces of the nodes (actual node floor reaction forces) in the state shown in FIG. 3(b) are illustratively shown in FIG. 11. In the diagram, a vector Fn_act (n=1,2,3,4,14,23) denotes a translational force component of an actual n-th node floor reaction force. Further, Ftotalact denotes the translational force component of an actual total floor reaction force (=actual 1423rd node floor reaction force). In general, the translational force component of the actual floor reaction force of each node that is not a leaf node is the resultant force of the translational force components of the actual floor reaction forces of all child nodes of the node. Accordingly, the translational force components of the actual floor reaction forces of the 14th node, the 23rd node, and the 1423rd node are F14act=F1act+F4act, F23act=F2act+F3act, Ftotalact(=F1423act) =F14act+F23act. The translational force components F1act, F2act, F3act and F4act of the actual floor reaction forces of the leaf nodes are the translational force components of the actual floor reaction forces (actual ground contact portion floor reaction forces) of the ground contact portions 10 obtained by the actual floor reaction force detector 108. The vectors shown by dashed lines in FIG. 11 indicate the translational force components of the desired node floor reaction forces shown in FIG. 10 mentioned above. The hierarchical compliance operation determiner 114 determines the translational force components of the actual node floor reaction forces of the nodes from the actual floor reaction forces of the ground contact portions 10 obtained by the actual floor reaction force detector 108.

The moment components of the actual floor reaction forces of the nodes in the state shown in FIG. 3(b) are illustratively shown in FIG. 12. In the diagram, a vector Mn_act (n=14,23,1423) denotes the moment component of an actual n-th node floor reaction force. In this case, as with the translational force components of the actual floor reaction forces of the nodes, in general, the moment component of an actual floor reaction force M14act, M23act or M1423act (=Mtotalact) of a node that is not a leaf node is defined as the moment component of the resultant force of the actual floor reaction forces of all child nodes of the node (the moment component having the desired floor reaction force central point Q14, Q23 or Q1423 of the node as the point of action).

In the robot 1 of the present embodiment, no floor reaction force moment can be generated in the ground contact portions 10 (leaf nodes), as described above, so that the moment components of the actual floor reaction forces of leaf nodes (actual ground contact portion floor reaction forces) will be always zero. Thus, the moment components of the actual node floor reaction forces of the leaf nodes will not be shown.

The actual floor reaction force moment components of nodes that are not leaf nodes (in the present embodiment, M14act, M23act and M1423act) are not generally zero. For example, a moment is usually generated about the desired 14th node floor reaction force central point Q14 by the horizontal components of the translational force components of the actual floor reaction forces of the first ground contact portion 10 (the 1st node) and the fourth ground contact portion 10 (the 4th node). However, in the robot 1 in the present embodiment, the distal end portions of the legs are provided with the spherical joints 12, which are free joints, so that a component in the same direction as a segment Q1Q4 of M14act and a component in the same direction as a segment Q2Q3 of M23act will be zero.

In a robot having the distal joints of its legs provided with actuators (a robot in which the postures of the distal ground contact portions of the legs are controllable), even if the control related to the rotation of the posture of the robot about the vertical axis is not carried out, the desired floor reaction force moment horizontal components of the ground contact portions are also set and the actual floor reaction force moments of the ground contact portions are also detected, as shown in an embodiment disclosed in Japanese Unexamined Patent Application Publication No. H10-277969 previously proposed by the present applicant. Then, an operation for correcting the postures of the ground contact portions should be performed so as to approximate the actual floor reaction force moment horizontal component of each ground contact portion to a desired floor reaction force moment horizontal component or to approximate the actual floor reaction force moment of each ground contact portion to the sum (the vector sum) of the desired floor reaction force moment horizontal component and its ground contact portion compensating floor reaction force moment.

The processing of the hierarchical compliance operation determiner 114 will be explained in more detail. In this case, a situation is assumed in which the posture of the body 24 of the robot 1 is about to fall toward left rear in the state shown in FIG. 3(b) and the compensating total floor reaction force moment Mdmd determined by the posture stabilization control calculator 104 is as shown in FIG. 13.

To restore the posture (the inclination relative to the vertical direction) of the body 24 of the robot 1 (to restore the inclination in a desired body posture), the horizontal component of an actual total floor reaction force moment about a desired total floor reaction force central point (a desired ZMP) may be made to follow the horizontal component of the sum of a desired total floor reaction force moment Mtotalref (=M1423ref) and a compensating total floor reaction force moment Mdmd.

Meanwhile, at the desired total floor reaction force central point (the desired ZMP), the horizontal component of the desired total floor reaction force moment Mtotalref is zero. Hence, to restore the posture (inclination) of the body 24 of the robot 1 in the longitudinal and lateral directions, the horizontal component of the actual total floor reaction force moment about the desired total floor reaction force central point (the desired ZMP) may be made to follow the horizontal components (Mdmdx, Mdmdy) of Mdmd. Further, in the present embodiment, the actual floor reaction force moment about the desired floor reaction force central point of each ground contact portion 10 is zero.

Therefore, the hierarchical compliance operation determiner 114 in the first embodiment corrects the desired ground contact portion position (especially the position in the height direction) of each ground contact portion 10 determined by the gait generating device 100 so as to satisfy the aforesaid requirements 1) and 2) as much as possible.

To make the correction, the hierarchical compliance operation determiner 114 determines a compensating angle. The compensating angle is a manipulated variable (a rotational amount) for correcting the relative relationship among the positions of the ground contact portions 10 by the operation of rotation about a certain point (correcting in the vertical direction in the present embodiment). In the present embodiment, there are a 14th node compensating angle θ14, a 23rd node compensating angle θ23, and a 1423rd node compensating angle θ1423 as the compensating angles. In other words, they are the compensating angles of the nodes other than leaf nodes. Then, the hierarchical compliance operation determiner 114 determines a desired n-th node floor reaction force central point Qn′ obtained by correcting the desired n-th node floor reaction force central point Qn (n=1,2,3,4,14,23) on the basis of these compensating angles θ14, θ23 and θ1423.

As shown in FIG. 15, the 14th node compensating angle θ14 is the angle formed by a segment Q1Q4 and a segment Q1′ Q4′, and the 23rd node compensating angle θ23 is the angle formed by a segment Q2Q3 and a segment Q2′Q3′. Further, as shown in FIG. 16, the 1423rd node compensating angle θ1423 is the angle formed by a segment Q14Q23 and a segment Q14′Q23′. The technique for determining these node compensating angles θ14, θ23 and θ1423 will be described later.

Referring to FIG. 15 and FIG. 16, the correction of the desired n-th node floor reaction force central point Qn (n=1,2,3,4,14,23) on the basis of the compensating angles θ14, θ23 and θ1423 is made as follows.

Referring to FIG. 15, a normal vector V14 of a plane that includes a desired first node floor reaction force central point Q1 (the desired floor reaction force central point of a first ground contact portion 10) and a desired fourth node floor reaction force central point Q4 (the desired floor reaction force central point of a second ground contact portion 10) and that is perpendicular to a horizontal plane is determined. The magnitude of V14 is 1. The coordinate (position) of the desired first node floor reaction force central point Q1 is rotationally moved about a normal vector V14 with the desired 14th node floor reaction force central point Q14 as the center of rotation (about the axis that passes Q14 and is parallel to V14) by the aforesaid 14th node compensating angle θ14. The point after Q1 is moved by the above rotational movement is defined as Q1′ . Similarly, the coordinate (position) of the desired fourth node floor reaction force central point Q4 is rotationally moved about the normal vector V14 with the desired 14th node floor reaction force central point as the center of rotation by the 14th node compensating angle θ14. The point after Q4 is moved by the above rotational movement is defined as Q4′. In other words, the ends of a segment obtained by rotating the segment Q1Q4 about V14 by θ14 with Q14, which is an internally dividing point thereof, as the center of rotation are defined as Q1′ and Q4′. Thus, the 14th node compensating angle θ14 is a manipulated variable for moving the relative relationship of the positions of the desired floor reaction force central points Q1 and Q4 of the first node and the fourth node, respectively, which are child nodes of the 14th node, without moving the position of the desired floor reaction force central point Q14 of the 14th node.

Moreover, a normal vector V23 of a plane that includes a desired second node floor reaction force central point Q2 and a desired third node floor reaction force central point Q3 and that is perpendicular to a horizontal plane is determined. The magnitude of V23 is 1. The coordinate (position) of the desired second node floor reaction force central point Q2 is rotationally moved about the normal vector V23 with the desired 23rd node floor reaction force central point Q23 as the center of rotation (about the axis that passes Q23 and is parallel to V23) by the 23rd node compensating angle θ23. As shown in FIG. 15, the point after Q2 is moved by the above rotational movement is defined as Q2′. Similarly, the coordinate (position) of the desired third ground contact portion floor reaction force central point Q3 is rotationally moved about the normal vector V23 with the desired 23rd node floor reaction force central point as the center of rotation by the 23rd node compensating angle θ23. The point after Q3 is moved by the above rotational movement is defined as Q3′. In other words, the ends of a segment obtained by rotating the segment Q2Q3 about V23 by θ23 with Q23, which is an internally dividing point thereof, as the center of rotation are defined as Q2′ and Q3′. Thus, the 23rd node compensating angle θ23 is a manipulated variable for moving the relative relationship of the positions of the desired floor reaction force central points Q2 and Q3 of the second node and the third node, respectively, which are child nodes of the 23rd node, without moving the position of the desired floor reaction force central point Q23 of the 23rd node.

Next, referring to FIG. 16, a normal vector V1423 of a plane that includes a desired 14th node floor reaction force central point Q14 and a desired 23rd node floor reaction force central point Q23 and that is perpendicular to a horizontal plane is determined. The magnitude of V1423 is 1. The coordinate (position) of the desired 14th node floor reaction force central point Q14 is rotationally moved about the normal vector V1423 with the desired total floor reaction force central point P (=Q1423) as the center of rotation (about the axis that passes P and is parallel to V1423) by the 1423rd compensating angle θ1423. The point after Q14 is moved by the above rotational movement is defined as Q14′. Similarly, the coordinate (position) of the desired 23rd ground contact portion floor reaction force central point Q23 is rotationally moved about the normal vector V1423 with the desired total floor reaction force central point P as the center of rotation by the 1423rd compensating angle θ1423. The point after Q23 is moved by the above rotational movement is defined as Q23′. In other words, the ends of a segment obtained by rotating a segment Q14Q23 about V1423 by 01423 with P(=Q1423), which is an internally dividing point thereof, as the center of rotation are defined as Q14′ and Q23′. Thus, the 1423rd node compensating angle θ1423 is a manipulated variable for moving the relative relationship of the positions of the desired floor reaction force central points Q14 and Q23 of the 14th node and the 23rd node, respectively, which are child nodes of the 1423rd node, without moving the position of the desired floor reaction force central point P of the 1423rd node.

Hereinafter, generally, a vector whose start point is A and end point is B will be denoted as a vector A_B.

Next, a point Q1″ that would be obtained when Q1′ is moved for a vector Q14_Q14′ is determined. Similarly, a point Q4″ that would be obtained when Q4′ is moved for a vector Q14_Q14′ is determined. Further, a point Q2″ that would be obtained when Q2′ is moved for a vector Q23_Q23′ is determined. Similarly, a point Q3″ that would be obtained when Q3′ is moved for a vector Q23_Q23′ is determined.

Subsequently, the desired ground contact portion position of an n-th ground contact portion (n=1,2,3,4) is moved in parallel by a vector Qn_Qn″ (substantially vertical movement). This corrects the desired ground contact portion position of each ground contact portion 10. Supplementally, as with the desired floor reaction force central point of each node, if the position of each leaf node is set to the position of the ground contact portion 10 corresponding to the leaf node, and the position of each node having child nodes is defined as a weighted average position of the positions of all child nodes of the node, using the weights set as described above, then the processing for moving the desired floor reaction force central points of the child nodes of the 14th node, the 23rd node and the 1423rd node, respectively, according to the compensating angles θ14, θ23 and θ1423, as described above, is equivalent to moving the positions of the child nodes of the 14th node, the 23rd node and the 1423rd node, respectively, according to the compensating angles θ14, θ23 and θ1423.

In a robot in which the distal end portions of its legs have no free joints (a robot in which the postures of ground contact portions can be controlled), the aforesaid parallel movement is performed without changing the postures (desired postures) of ground contact portions, and then the n-th ground contact portion is further rotated by a certain rotational angle θn_x about a longitudinal axis (X axis) and also rotated about a lateral axis (Y axis) by a certain rotational angle θn_y, with Qn″ being the center. This corrects the desired postures of ground contact portions. The rotational angle θn (a two-dimensional amount composed of θn_x and θn_y) in this case is referred to as an n-th ground contact portion compensating angle, θn_x is referred to as an n-th ground contact portion compensating angle X component and θn_y is referred to as an n-th ground contact portion compensating angle Y component. The n-th ground contact portion compensating angle θn may be determined according to the technique described in Japanese Unexamined Patent Application Publication No. H10-277969 previously proposed by the present applicant.

Supplementally, each of the node compensating angles is determined such that a resultant force of a desired floor reaction force moment (the horizontal component thereof is zero) and a node compensating floor reaction force moment Mn_dmd is generated at the desired node floor reaction force central point of the node, which is the point of action. In this case, the compensating total floor reaction force moment Mdmd, which provides the basis of the node compensating floor reaction force moment Mn_dmd, is determined so as to approximate an actual posture inclination error to zero. Hence, each node compensating angle will function as a manipulated variable for manipulating the relative positional relationship among the ground contact portions 10 so as to approximate an actual posture inclination error to zero while approximating a floor reaction force moment about a desired total floor reaction force central point to a desired moment (a compensating total floor reaction force moment in this case).

As described above, the processing for correcting the desired ground contact portion positions (specifically, the mutual relative relationship among the positions) of the ground contact portions 10 is the hierarchical compliance operation in the first embodiment. In the compliance operation, for each node having child nodes, the manipulated variable (correction amount) of the relative relationship among the desired ground contact portion positions (relative positional relationship) of the ground contact portions 10, which are the descendant nodes of the node, is established for each of the compensating angles θ14, θ23 and θ1423, and the manipulated variables (correction amounts) are combined so as to correct the mutual relative relationship of the desired ground contact portion positions of the ground contact portions 10. The desired ground contact portion position/posture corrected as described above is referred to as a corrected desired ground contact portion position/posture.

In such a compliance operation, generally, if the aforesaid compensating angles (compensating operation amounts) are not excessive, then the ground contact region (a region of the ground contact surface wherein the pressure is positive) remains unchanged even if the ground contact pressure distribution of each ground contact portion 10 changes. In this case, the compliance mechanism 42 attached to each ground contact portion 10 deforms in proportion to a compensating angle, and an actual floor reaction force of each ground contact portion 10 based on the amount of the deformation is generated. As a result, the relationship between the compensating angles and the changing amounts of the actual floor reaction forces generated by the compensating angles has good characteristics, namely, linear characteristics, shown below.

Characteristic 1) If only the compensating angles about the desired floor reaction force central points of nodes that are not leaf nodes are manipulated to move the desired ground contact portion positions of the ground contact portions 10, then the translational force components of the actual floor reaction forces of the ground contact portions 10 that have been lowered increase, while the translational force components of the actual floor reaction forces of the ground contact portions 10 that have been raised decrease. At this time, the actual floor reaction force moments about the corrected desired floor reaction force central point of the ground contact portions 10 (leaf nodes) hardly change.

Characteristic 2) If only an n-th ground contact portion compensating angle is manipulated to rotate the desired posture of the n-th ground contact portion, then the moment component of the actual floor reaction force acting on the desired floor reaction force central point of the n-th ground contact portion changes, while the translational force component does not change much.

Characteristic 3) If the compensating angle about the desired floor reaction force central point of a node that is not a leaf node and an n-th ground contact portion compensating angle are simultaneously manipulated, then the change amount of the actual floor reaction forces of the ground contact portions 10 will be equivalent to the sum of the change amount observed when each of them is independently manipulated.

FIG. 17 is a block diagram showing the functions of the hierarchical compliance operation determiner 114 in the present embodiment. Referring to the diagram, the processing of the hierarchical compliance operation determiner 114 will be explained in further detail.

The hierarchical compliance operation determiner 114 is equipped with, as its functional means, a compensating total floor reaction force moment distributor 114 a, compensating angle determiners 114 b, 114 c and 114d, a corrected desired ground contact portion position/posture calculator 114 g, a deformation compensation amount calculator 114 n, and a corrected desired ground contact portion position/posture with deformation compensation calculator 114 h.

The compensating total floor reaction force moment distributor 114 a distributes the aforesaid compensating total floor reaction force moment Mdmd (Mdmdx, Mdmdy) to the 1423rd node compensating floor reaction force moment M1423 dmd, the 14th node compensating floor reaction force moment M14 dmd and the 23rd node compensating floor reaction force moment M23 dmd.

The 1423rd node compensating floor reaction force moment M1423 dmd is the desired value of a moment to be generated about the desired 1423rd node floor reaction force central point (=desired total floor reaction force central point (desired ZMP)) by the translational force components of the floor reaction forces of the ground contact portions 10 (specifically, the first to the fourth ground contact portions) when the 1423rd compensating angle θ1423 is manipulated.

The component of the 1423rd node compensating floor reaction force moment M1423 dmd in the direction of the aforesaid vector V1423 (the component about the axis in the V1423 direction) is described as M1423 dmdv. The vector V1423 is the vector defined in the general explanation of the compliance operation of the hierarchical compliance operation determiner 114 (refer to FIG. 16). If a vector orthogonal to V1423 and also orthogonal to the vertical direction is defined as U1423, then the component in the direction of U1423 of the 1423rd node compensating floor reaction force moment M1423 dmd (the component about the axis in the U1423 direction) M1423 dmdu is set to zero in the present embodiment. This is because the component of a floor reaction force moment in the U1423 direction cannot be generated even if the 1423rd node compensating angle θ1423 is manipulated in the robot 1 of the present embodiment. Further, in the present embodiment, the rotation of the posture of the robot 1 about the vertical axis is not controlled; therefore, the component M1423 dmdz of M1423 dmd in the vertical direction is also set to zero.

The 14th node compensating floor reaction force moment M14 dmd is the desired value of a moment to be generated about the desired 14th node floor reaction force central point by the translational force components of the floor reaction forces of the ground contact portions 10 (specifically, the first and the fourth ground contact portions) when the 14th compensating angle θ14 is manipulated.

The component of the 14th node compensating floor reaction force moment M14 dmd in the direction of the vector V14 is described as M14 dmdv. The vector V14 is the vector defined in the general explanation of the compliance operation of the hierarchical compliance operation determiner 114 (refer to FIG. 15). If a vector orthogonal to V14 and also orthogonal to the vertical direction is defined as U14, then the component M14 dmdu in the direction of U14 of the 14th node compensating floor reaction force moment M14 dmd is set to zero in the present embodiment. This is because the component of a floor reaction force moment in the U14 direction cannot be generated even if the 14th node compensating angle θ14 is manipulated in the robot 1 of the present embodiment. In the present embodiment, the component of M14 dmd in the vertical direction is also set to zero.

The 23rd node compensating floor reaction force moment M23 dmd is the desired value of a moment to be generated about the desired 23rd node floor reaction force central point by the translational force components of the floor reaction forces of the ground contact portions 10 (specifically, the second and the third ground contact portions) when the 23rd compensating angle θ23 is manipulated.

The component of the 23rd node compensating floor reaction force moment M23 dmd in the direction of the vector V23 is described as M23 dmdv. The vector V23 is the vector defined in the general explanation of the compliance operation of the hierarchical compliance operation determiner 114 (refer to FIG. 15). If a vector orthogonal to V23 and also orthogonal to the vertical direction is defined as U23, then the component M23 dmdu in the direction of U23 of the 23rd node compensating floor reaction force moment M23 dmd is set to zero in the present embodiment. This is because the component of a floor reaction force moment in the U23 direction cannot be generated even if the 23rd node compensating angle θ23 is manipulated in the robot 1 of the present embodiment. In the present embodiment, the component of M23 dmd in the vertical direction is also set to zero.

The 1423rd node compensating floor reaction force moment M1423 dmd, the 14th node compensating floor reaction force moment M14 dmd and the 23rd node compensating floor reaction force moment M23 dmd are determined, for example, as follows.

On an arbitrary n-th node, when a desired n-th node floor reaction force central point has been corrected while maintaining the horizontal component of an n-th node compensating floor reaction force moment at zero so as to be equivalent to adding the n-th node compensating floor reaction force moment to the desired n-th node floor reaction force central point, the desired n-th node floor reaction force central point position that has been corrected is referred to as a corrected desired n-th node floor reaction force central point position.

The relationships between a corrected desired 1423rd node floor reaction force central point Pmdfd (=Q1423 mdfd), a corrected desired 14th node floor reaction force central point Q14mdfd, and a corrected desired 23rd node floor reaction force central point Q23 mdfd, and node compensating floor reaction force moments are shown in the following expressions 7 to 9. The desired 1423rd node floor reaction force central point Q1423 agrees with the total floor reaction force central point P, so that the corrected desired 1423rd node floor reaction force central point Pmdfd may be referred to as a corrected desired total floor reaction force central point in some cases. M1423dmd=(Pmdfd−P)*Ftotalref   Expression 7 M14dmd=(Q14mdfd−Q14)*F14ref   Expression 8 M23dmd=(Q23mdfd−Q23)*F23ref   Expression 9

The difference between two points, such as (Pmdfd−P) in expression 7, means the difference in the positional vectors of the points. Further, Ftotalref, F14 ref and F23 ref denote the translational force components of the desired node floor reaction forces of the 1423rd node, the 14th node and the 23rd node, respectively, as shown in FIG. 10.

To maintain high ground contact properties of the robot 1, control should not be carried out to bring the floor reaction force of the ground contact portion 10 to be in contact with the ground excessively close to zero or a negative value. Hence, the following conditions 1) to 3) should be satisfied.

Corrected Node Existing Position Condition 1)

Pmdfd is not excessively close to an end of the segment Q14Q23 from the total floor reaction force central point P and exists on the segment Q14Q23. The range in which the corrected desired 1423rd node floor reaction force central point (corrected desired total floor reaction force central point) Pmdfd should exist is referred to as the existence permissible range of the corrected desired 1423rd node floor reaction force central point (corrected desired total floor reaction force central point).

Corrected Node Existing Position Condition 2)

Q14 mdfd is not excessively close to an end of the segment Q1Q4 from the desired 14th node floor reaction force central point Q14 and exists on the segment Q1Q4. The range in which the corrected desired 14th node floor reaction force central point Q14 mdfd should exist is referred to as the existence permissible range of the corrected desired 14th node floor reaction force central point.

Corrected Node Existing Position Condition 3)

Q23 mdfd is not excessively close to an end of the segment Q2Q3 from the desired 23rd node floor reaction force central point Q23 and exists on the segment Q2Q3. The range in which the corrected desired 23rd node floor reaction force central point Q23 mdfd should exist is referred to as the existence permissible range of the corrected desired 23rd node floor reaction force central point.

Meanwhile, to obtain an appropriate posture restoring force actually generated in the robot 1 (the force for restoring the inclination of the body 24 to a desired body posture inclination), the resultant force of the 1423rd node compensating floor reaction force moment M1423 dmd, the 14th node compensating floor reaction force moment M14 dmd and the 23rd node compensating floor reaction force moment M23 dmd should approximately agree with the compensating total floor reaction force moment Mdmd. In other words, the following expression 10 should be approximately satisfied. Mdmd=M1423dmd+M14dmd+M23dmd   Expression 10

In the present embodiment, therefore, the 1423rd node compensating floor reaction force moment M1423 dmd, the 14th node compensating floor reaction force moment M14 dmd and the 23rd node compensating floor reaction force moment M23 dmd are determined according to expressions 11 to 13 given below as long as the positions of the corrected desired node floor reaction force central points Pmdfd, Q14 mdfd and Q23 mdfd determined by the above and the aforesaid expression 7, expression 8 and expression 9 satisfy the aforesaid corrected node existence position conditions 1), 2) and 3). M1423dmd=Mat1423*Mdmd*V1423   Expression 11 M14dmd=Mat14*Mdmd*V14   Expression 12 M23dmd=Mat23*Mdmd*V23   Expression 13 where Mat1423, Mat14 and Mat23 denote gain matrixes (1-row, 3-column matrixes with a third element being zero), and these are set such that M1423 dmd, M14 dmd and M23 dmd determined according to expression 11, expression 12 and expression 13 satisfy expression 10.

More precisely, the gain matrixes Mat1423, Mat14 and Mat23 are determined such that a simultaneous equation composed of expression 10, expression 11, expression 12 and expression 13 identically holds regardless of the value of Mdmd. The gain matrix for the simultaneous equation to identically hold is not uniquely determined, so that an appropriate gain matrix may be determined depending on, for example, which one of the corrected node existence position conditions 1), 2) and 3) is especially important. Supplementally, the gain matrixes are desirably changed continuously in order to continuously change the compensating angles θ1423, θ14 and θ23. Further, the policy of setting gain matrixes may be changed according to whether the robot 1 is standing upright or the difference in movement mode or the like.

If the 1423rd node compensating floor reaction force moment M1423 dmd, the 14th node compensating floor reaction force moment M14 dmd and the 23rd node compensating floor reaction force moment M23 dmd determined according to expression 11, expression 12 and expression 13 do not satisfy any one of the corrected node existence position conditions 1), 2) and 3), then they are to be corrected to satisfy the conditions 1), 2) and 3). More specifically, of the corrected desired n-th node floor reaction force central points (n=1423, 14, 23), the corrected desired node floor reaction force central point that has exceeded the existence permissible range is set on the boundary of the existence permissible range, and the remaining corrected desired node floor reaction force central points are determined so as to satisfy expression 11, expression 12 and expression 13 as much as possible (to minimize the absolute value of the difference between a left side and a right side as much as possible). Generally, however, the behavior of a controlled object does not considerably change even if a feedback amount slightly changes in feedback control; therefore, it is not required to strictly or forcibly satisfy expression 11, expression 12 and expression 13.

Thus, the compensating total floor reaction force moment distributor 114 a determines the node compensating floor reaction force moments M1423 dmd, M14 dmd and M23 dmd. FIG. 14 shows the examples of the node compensating floor reaction force moments M1423 dmd, M14 dmd and M23 dmd determined as described above. In the figure, Mdmd is the same as that shown in FIG. 13 mentioned above.

Moreover, the compensating total floor reaction force moment distributor 114 a determines the corrected desired ground contact portion floor reaction forces Fn_refmdfd (n=1, 2, 3, 4), which are the desired floor reaction forces of the ground contact portions 10 that are corrected by adding node compensating floor reaction force moments to desired ground contact portion floor reaction forces, on the basis of the desired ground contact portion floor reaction forces, which are the desired floor reaction forces of the ground contact portions 10 (the desired node floor reaction forces of leaf nodes) and the node compensating floor reaction force moments M1423 dmd, M14 dmd and M23 dmd.

At this time, the corrected desired ground contact portion floor reaction forces Fn_refmdfd (n=1, 2, 3, 4) may be determined from the desired floor reaction force central points Qn(n=1, 2, 3, 4), the corrected total floor reaction force central points Pmdfd, the corrected desired node floor reaction force central points Q14 mdfd and Q23 mdfd, and the desired total floor reaction forces Ftotalref of the ground contact portions 10 by the same technique as the technique for determining deciding desired node floor reaction forces from the desired floor reaction force central points (the desired node floor reaction force central points of leaf nodes) Qn(n=1, 2, 3, 4), the desired total floor reaction force central points P, the desired node floor reaction force central points Q14 and Q23, and the desired total floor reaction forces Ftotalref of the ground contact portions 10. In other words, the weights of the nodes are determined according to the aforesaid expressions 1 to 3 from the desired floor reaction force central points Qn(n=1, 2, 3, 4), the corrected total floor reaction force central points Pmdfd, the corrected desired node floor reaction force central points Q14 mdfd and Q23 mdfd of the ground contact portions 10, and the corrected desired ground contact portion floor reaction forces Fn_refmdfd (n=1, 2, 3, 4) may be determined according to the aforesaid expression 4 by using the determined weight.

As is obvious from the above, generating a node compensating floor reaction force moment at a desired n-th node floor reaction force central point of an n-th node having child nodes (correcting the moment component of the desired floor reaction force acting on a desired n-th node floor reaction force central point) is equivalent to correcting the weights of the child nodes of the n-th node.

The processing of the compensating total floor reaction force moment distributor 114 a explained above is represented by the functional block diagram shown in FIG. 18. More specifically, based on the desired node floor reaction force central points Qn(n=1, 2, 3, 4, 14, 23), the existence permissible ranges of the 14th node, the 23rd node and the corrected desired node floor reaction force central points Qn_mdfd (n=14, 23, 1423) are determined according to the aforesaid corrected node existence position conditions 1), 2) and 3). Further, the corrected desired node floor reaction force central points Qn_mdfd (n=14, 23, 1423) and the node compensating floor reaction force moments Mn_dmd(n=14, 23, 1423) are determined on the basis of the compensating total floor reaction force moment Mdmd, the desired floor reaction force central points of nodes Qn(n=1, 2, 3, 4, 14, 23, 1423), the desired floor reaction forces Fn_ref(n=1, 2, 3, 4, 14, 23, 1423) and the aforesaid existence permissible ranges. In addition, the corrected desired ground contact portion floor reaction forces Fn_refmdfd (n=1, 2, 3, 4) are determined on the basis of the corrected desired node floor reaction force central points Qn_mdfd(n=14, 23, 1423), the desired floor reaction force central points Qn(n=1, 2, 3, 4) of the ground contact portions 10, and the desired total floor reaction force Ftotalref.

Next, the processing of the compensating angle determiner (θ1423 determiner) 114 b out of the aforesaid compensating angle determiners 114 b to 114 d will be explained with reference to the block diagram of FIG. 19. If the translational force component (F1 act+F4 act) of the resultant force of the actual first node floor reaction force and the actual fourth node floor reaction force acts on a desired 14th node floor reaction force central point Q14 and the translational force component (F2 act+F3 act) of the resultant force of the actual second node floor reaction force and the actual third node floor reaction force acts on a desired 23rd node floor reaction force central point Q23, a moment M1423 act generated about the desired total floor reaction force central point P (the desired 1423rd node floor reaction force central point) by the above translational force components is determined according to expression 14 given below. M1423act=P _(—) Q14*(F1act+F4act)+P _(—) Q23*(F2act+F3act)   Expression 14 where P_Q14 denotes a vector whose start point is P and end point is Q14, and P_Q23 denotes a vector whose start point is P and end point is Q23.

In actuality, there will be hardly a problem if expression 15 given below is used in place of expression 14 to calculate M1423 act. $\begin{matrix} {{M\quad 1423{act}} = {{{P\_ Q}\quad 1*F\quad 1{act}} + {{P\_ Q2}*F\quad 2{act}} + {{P\_ Q}\quad 3*F\quad 3{act}} + {{P\_ Q4}*F\quad 4{act}}}} & {{Expression}\quad 15} \end{matrix}$

The right side of expression 15 is an expression for calculating the actual total floor reaction force moment Mtotalact acting about the desired total floor reaction force central point P from the translational force components Fn_act(n=1, 2, 3, 4) of the actual floor reaction forces of the first to the fourth nodes (leaf nodes). Supplementally, in expression 14, the actual floor reaction force moment acting about the desired 14th node floor reaction force central point Q14 and the actual floor reaction force moment acting about the desired 23rd node floor reaction force central point Q23 are subtracted from the actual total floor reaction force moment Mtotalact acting about the desired total floor reaction force central point P.

In general, an actual n-th node floor reaction force moment Mn_act of an arbitrary n-th node, which is a leaf node, is the actual floor reaction force moment of an n-th ground contact portion. Regarding an arbitrary n-th node, which is not a leaf node, the moment from the floor reaction forces of all child nodes thereof acting on the desired n-th node floor reaction force central point (the floor reaction forces of the child nodes here are, strictly speaking, the floor reaction forces acting on the desired floor reaction force central points of the child nodes) is referred to as an actual n-th node floor reaction force moment Mn_act. As in expression 14 and expression 15, there are two different definitions of an actual n-th node floor reaction force moment. One includes the actual floor reaction force moments of child nodes and the other does not include them; however, either of the definitions may be used. Especially when the responsiveness of feedback control for controlling the horizontal components of the actual floor reaction force moments of child nodes is high, the horizontal components of the actual floor reaction force moments of the child nodes immediately converges to zero, so that the behavior of the control of the actual floor reaction force of an n-th node does not change much when either of the definitions is used.

Expression 16 shown below is a general expression for calculating an actual n-th node floor reaction force moment corresponding to expression 14, and expression 17 is a general expression for calculating an actual n-th node floor reaction force moment corresponding to expression 15. In expression 16 and expression 17, Qn_Qm is a vector whose start point is Qn and end point is Qm. An actual floor reaction force moment Mm_act of an m-th node, which is a leaf node, is an actual floor reaction force moment of each ground contact portion detected by an actual floor reaction force detector.

when mε {set of numbers of child nodes of n-th node}, Mn _(—) act=ΣY(Qn _(—) Qm*Fm _(—) act)   Expression 16 when mε {set of numbers of leaf nodes, which are descendants of n-th node}, Mn _(—) act=Y(Qn _(—) Qm*Fm_act+Mm _(—) act)   Expression 17

Σ in expressions 16 and 17 means the total sum regarding m. Supplementally, in the first embodiment, the actual floor reaction force moment about the desired floor reaction force central point of a leaf node (each ground contact portion) becomes zero; therefore, the right side of the above expression 15 does not include a component of Mm act of expression 17. In a robot in which the postures of ground contact portions can be controlled, Mm act of expression 17 generally does not become zero.

The 1423rd node compensating angle θ1423 may generally be determined by feedback control law or the like such that the difference between M1423 act determined as described above and the 1423rd node compensating floor reaction force moment M1423 dmd previously determined by the compensating total floor reaction force moment distributor 114 a (M1423 act−M1423 dmd) is approximated to zero. For example, θ1423 may be determined by, for example, multiplying the difference by a predetermined gain matrix (tertiary diagonal matrix).

However, in the present embodiment, the 1423rd node compensating angle θ1423 about the axis in the direction of the aforesaid vector V1423 may be determined, so that θ1423 may be determined on the basis of the difference between a component M1423 actv in the direction of the vector V1423 of M1423 act and a component M1423 dmdv in the direction of the vector V1423 of M1423 dmd. And, at this time, in the present embodiment, M1423 actv and M1423 dmdv are passed through a filter before determining θ1423 on the basis of the difference therebetween in order to enhance the responsiveness and stability of the control of floor reaction forces.

Specifically, the component M1423 actv in the direction of the vector V1423 of M1423 act determined as described above is extracted. This is obtained by the following expression 18 using a scalar product calculation “·” of a vector. M1423actv=M1423act·V1423   Expression 18

The processing for calculating M1423 actv as described above is executed by a calculator indicated by a reference numeral 114 k in FIG. 19.

Next, the aforesaid M1423 actv is passed through a low-pass filter 114 i to obtain M1423 actvfilt. Furthermore, the component M1423 dmdv in the V1423 direction of the aforesaid 1423rd node compensating floor reaction force moment M1423 dmd is passed through a compensating filter 114 j to obtain M1423 dmdvfilt. Incidentally, M1423 dmdv is determined by the scalar product calculation of M1423 dmd and V1423, as with the aforesaid expression 18. Then, the result obtained by subtracting M1423 dmdvfilt from M1423 actvfilt provides a component M1423 errv in the direction of an error moment V1423.

The aforesaid compensating filter 114 j improves the frequency response characteristic of transfer functions from M1423 dmdv to an actual total floor reaction force moment in a control device.

Lastly, the aforesaid 1423rd node compensating angle θ1423 is obtained by the calculation of the feedback control law (proportional control law in this case) of the following expression 19. Here, K1423 denotes a control gain, and it is normally set to a positive value. θ1423=K1423*M1423errv   Expression 19 This means that the component M1423 errv in direction of the error moment V1423 is multiplied by the control gain K1423 to obtain the 1423rd node compensating angle θ1423.

The processing of the compensating angle determiner (θ14 determiner) 114 c in FIG. 17 will now be explained with reference to FIG. 20. The translational force component Flact of the actual first node floor reaction force (the actual floor reaction force of the first ground contact portion 10) acts on a desired first node floor reaction force central point Q1 and the translational force component F4 act of the actual fourth node floor reaction force (the actual floor reaction force of the fourth ground contact portion 10) acts on a desired fourth node floor reaction force central point Q4, a moment M14 act generated about the desired 14th node floor reaction force central point Q14 by the above translational force components is determined according to the aforesaid expression 16, which is a general expression.

More specifically, it is determined according to the following expression 20. M14act=Q14_(—) Q1*F1act+Q14_(—) Q4*F4act   Expression 20 where Q14_Q1 denotes a vector whose start point is Q14 and end point is Q1, and Q14_Q4 denotes a vector whose start point is Q14 and end point is Q4.

In actuality, for the reason described previously, there will be hardly a problem if the following expression 21 (expression substantiating the above expression 17) is used in place of expression 20. M14act=Q14_(—) Q1*F1act+Q14_(—) Q4*F4act+M1act+M4act   Expression 21 where M1 act denotes an actual first node floor reaction force moment and M4 act denotes an actual fourth node floor reaction force moment. In the first embodiment, the distal portions of the legs #1 to #4 are provided with free joints (spherical joints 12), so that M1 act and M4 act are zero.

Expression 21 is an expression for calculating the moment acting about the desired 14th node floor reaction force central point generated by the resultant force of the actual floor reaction forces of all leaf nodes of the 14th node. Incidentally, expression 20 subtracts the actual first node floor reaction force moment and the actual fourth node floor reaction force moment from the moment acting about the desired 14th floor reaction force central point generated by the resultant force of the actual floor reaction forces of all leaf nodes of the 14th node.

The 14th node compensating angle θ14 may be generally determined by the feedback control law or the like so as to approximate the difference between M14 act determined as described above and the 14th node compensating floor reaction force moment M1423 dmd previously determined by the compensating total floor reaction force moment distributor 114 a (M14 act−M14 dmd) to zero.

In the present embodiment, however, for the same reason as that in the case of θ1423, θ14 is determined on the basis of the difference between the component M14 actv of M14 act in the aforesaid direction of the vector V14 and the component M14 dmdv of M14 dmd in the direction of the vector V14, which have been respectively passed through a filter.

More specifically, the component M14 actv of M14 act in the direction of the vector V14 determined as described above is extracted. This is obtained by expression 22 that uses the scalar product calculation of vectors. M14actv=M14act·V14   Expression 22 The processing for calculating M14 actv as described above is carried out by a calculator indicated by a reference character 114 k′ in FIG. 20.

Next, the aforesaid M14 actv is passed through a low-pass filter 114 i′ to obtain M14 actvfilt. Further, the component M14 dmdv of the aforesaid 14th node compensating floor reaction force moment in the direction of V14 is passed through a compensating filter 114 j′ to obtain M14 dmdvfilt. Incidentally, M14 dmdv is determined by the scalar product calculation of M14 dmd and V14. Then, M14 dmdvfilt is subtracted from M14 actvfilt to obtain a component M14 errv in the direction of an error moment V14. Incidentally, the compensating filter 114 j′ improves the frequency response characteristics of transfer functions from M14 dmdv to actual total floor reaction force moments in a control device.

Lastly, the aforesaid 14th node compensating angle θ14 is obtained by the calculation of the feedback control law (proportional control law in this case) of expression 23 given below. Here, K14 denotes a control gain, which is normally set to a positive value. θ14=K14*M14errv   Expression 23

The processing of the compensating angle determiner (θ23 determiner) 114 d in FIG. 17 is the same as the processing of the 14th node compensating angle θ14 determiner 114 c; therefore, detailed explanation thereof will be omitted here. The following provides an overview of the processing. A component M23 actv of a moment M23 act in the direction of a vector V23, the moment being generated about a desired 23rd node floor reaction force central point Q23 by the actual floor reaction forces of the child nodes of a 23rd node, is calculated on the basis of the aforesaid expression 16 or 17. Then, the 23rd node compensating angle θ23 is calculated by the arithmetic processing of the feedback control law (proportional control law) from a component M23 errv in the direction of the error moment V23 obtained by subtracting M23 actvfilt, which is obtained by passing the above M23 actv through a low-pass filter, from M23 dmdvfilt obtained by passing M23 dmdv of the aforesaid 23rd node compensating floor reaction force moment M23 dmd in the direction of V23 through a compensating filter.

The processing of the compensating angle determiners 114 b to 114 d determines the sets of the node compensating angles θ1423, θ14 and θ23 such that the horizontal components of the actual moments Mact acting on the desired total floor reaction force central points P are approximated to the compensating total floor reaction force moment Mdmd. In the present embodiment, θ1423 has been determined by the aforesaid expression 19; alternatively, however, it may be determined on the basis of the difference between M1423 act+M14 act+M23 act (the total sum of actual node floor reaction force moments) and M1423 dmd or the difference between the total sum of actual node floor reaction force moments and M1423 dmd, which have been respectively passed through a filter, in place of M1423 err of the right side of expression 19.

Supplementally, in the present embodiment, a node compensating floor reaction force moment has been determined without changing the desired node floor reaction force central point Qn of each node, the Qn being the point of action. Then, the difference between the node floor reaction force compensating moment (more precisely, the resultant force of the node compensating floor reaction force moment and the moment component of the desired node floor reaction force having Qn as its point of action) and an actual node floor reaction force moment having Qn as its point of action has been used as a control amount to determine a node compensating angle such that the control amount is approximated to zero. Instead of determining the node compensating angles as described above, the node compensating angles θ1423, θ14 and θ23 may be determined as follows. On each n-th node (n=14, 23, 1423) having child nodes, a floor reaction force central point that causes the horizontal component of the moment of the actual node floor reaction force of the n-th node (the resultant force of the actual node floor reaction forces of all child nodes of the n-th node) to become zero is determined as an actual n-th node floor reaction force central point. Alternatively, a floor reaction force central point that causes the horizontal component of a moment, which is obtained by subtracting the moment acting on the desired node floor reaction force central point of each child node due to the actual node floor reaction force of the child node from the moment of the actual node floor reaction force of an n-th node (n=14, 23, 1423), to become zero is determined as an actual n-th node floor reaction force central point. For example, the actual 14th node floor reaction force central point related to the 14th node is determined as a point obtained by shifting, on the segment Q1Q4, the desired 14th node floor reaction force central point by the value obtained by dividing the horizontal component of M14 act determined by the aforesaid expression 16 or 17 by the vertical component of the resultant force of F1 act and F4 act (the translational force component of an actual 14th node floor reaction force). The same applies to the node floor reaction force central points of the 23rd node and the 1423rd node. Then, the difference between the desired node floor reaction force central point of an n-th node (n=14, 23, 1423) and the actual n-th node floor reaction force central point determined as described above, or the difference between the values obtained by passing each of the above node floor reaction force central points through a filter is defined as a control amount, and the node compensating angles θ1423, θ14 and θ23 are determined on the basis of the control amount such that the control amount is approximated to zero (e.g., θ1423, θ14 and θ23 are determined by multiplying the control amount by a certain gain).

The corrected desired ground contact portion position/posture calculator 114 g in FIG. 17 obtains corrected desired ground contact portion position/posture by correcting the desired ground contact portion position/posture, which are the desired position and the desired posture of each ground contact portion 10, according to a technique for correcting the desired ground contact portion position/posture of the aforesaid hierarchical compliance operation (the technique explained with reference to FIG. 15 and FIG. 16) on the basis of the 1423rd node compensating angle θ1423, the 14th node compensating angle θ14, and the 23rd node compensating angle θ23. However, in the present embodiment, the distal portions of the legs #1 to #4 are provided with free joints (spherical joints 12), and the posture of each ground contact portion 10 cannot be intentionally changed, so that the corrected desired ground contact portion position/posture actually means the corrected desired ground contact portion position.

FIG. 21 is a functional block diagram showing the processing of the aforesaid deformation compensation amount calculator 114 n in FIG. 17. As shown in FIG. 21, the deformation compensation amount calculator 114 n determines deformation amounts En_mdfd(n=1, 2, 3, 4), which are the deformation amounts of the legs #1 to #4 (the deformation amounts of the link mechanisms of the legs and the compliance mechanism 42), the deformation being expected to occur due to the corrected desired ground contact portion floor reaction forces Fn_refmdfd (n=1, 2, 3, 4) of the ground contact portions 10 that are output from the compensating total floor reaction force moment distributor 114 a (the desired ground contact portion floor reaction forces that have been corrected by adding the 1423rd node compensating floor reaction force moment M1423 dmd, the 14th node compensating floor reaction force moment M14 dmd and the 23rd node compensating floor reaction force moment M23 dmd to desired ground contact portion floor reaction forces). The deformation amounts En_mdfd(n=1, 2, 3, 4) are determined by using a mechanism compliance model showing the relationship between the forces (or floor reaction forces) acting on the legs and the deformations of the legs. Then, the deformation compensation amount calculator 114 n determines deformation compensation amounts En_cmpn(n=1, 2, 3, 4) for canceling the deformation amounts En_mdfd(n=1, 2, 3, 4). The deformation compensation amount En_cmpn is determined by multiplying each deformation amount En_mdfd by (−1).

The corrected desired ground contact portion position/posture with deformation compensation calculator 114h in FIG. 17 adds a further correction to the corrected desired ground contact portion position/posture of each ground contact portion 10 (the position/posture determined by the aforesaid corrected desired ground contact portion position/posture calculator 114 g) so as to cancel the calculated deformation amount En_mdfd, thereby obtaining the corrected desired ground contact portion position/posture with deformation compensation of each ground contact portion 10. The corrected desired ground contact portion positions/postures with deformation compensation are determined by adding individually corresponding deformation compensation amounts En_cmpn to the corrected desired ground contact portion position/posture of the ground contact portions 10.

For example, if it is calculated that a corrected desired ground contact portion floor reaction force causes the compliance mechanism 42 or the like of a leg corresponding to a certain ground contact portion 10 to contract by z in the vertical direction, then the corrected desired ground contact portion floor reaction force is corrected so as to lower the desired position of the ground contact portion 10 by z. More specifically, the corrected desired ground contact portion position/posture with deformation compensation is calculated such that the position/posture when the ground contact surface (bottom surface) of the ground contact portion 10 after deformation compensation is deformed under a desired ground contact portion floor reaction force agrees with the desired position/posture of the ground contact surface of the ground contact portion before the deformation compensation. Incidentally, detailed explanation thereof is explained in detail in Japanese Unexamined Patent Application Publication No. H10-277969. In the robot 1 of the present embodiment, the postures of the ground contact portions 10 cannot be controlled, so that the corrected desired ground contact portion position/posture with deformation compensation calculator 114 h actually corrects the corrected desired ground contact portion positions of the ground contact portions 10.

The deformation compensation described above is implemented by control for canceling in a feed-forward manner the shifts in actual ground contact portion position/posture caused by a deformation of the compliance mechanism 42 or the like. In comparison with a case where no such control is carried out, it is possible to enable the robot 1 to travel in a gait that is further closer to a desired gait.

The above has given the details of the processing of the hierarchical compliance operation determiner 114.

Based on the above, resuming the explanation of the flowchart in FIG. 9, compensating angles are determined as described above in S34. FIG. 22 is a flowchart showing a generalized subroutine of the processing for determining the compensating angles.

To explain with reference to the figure, first, the translational force components Fn_act(n=1, 2, 3, 4, 14, 23, 1423) of actual n-th node floor reaction forces are calculated on the basis of the actual floor reaction forces of the ground contact portions 10 (the actual floor reaction forces obtained by the aforesaid actual floor reaction force detector 108). In this case, the translational force components Fn_act(n=1, 2, 3, 4) of the actual node floor reaction forces of the leaf nodes are the translational force components of the detected values of the actual floor reaction forces of the ground contact portions 10 by the aforesaid actual floor reaction force detector 108. Further, the translational force component Fn_act(n=14, 23, 1423) of the actual node floor reaction force of a node that is not a leaf node is the translational force component of the resultant force of the actual node floor reaction forces of the child nodes of the node, as described above.

Subsequently, the processing proceeds to S102 to calculate an actual n-th node floor reaction force moment Mn_act (n=1, 2, 3, 4, 14, 23, 1423). In the robot 1 of the first embodiment, the actual node floor reaction force moment Mnnact (n=1, 2, 3, 4) of each leaf node is zero. Further, the actual node floor reaction force moment Mn_act(n=14, 23, 1423) of each node that is not a leaf node is calculated on the basis of the aforesaid expression 16 or expression 17 from Fn_act(n=1, 2, 3, 4) obtained in S100 and the desired node floor reaction force central point Qn(n=1, 2, 3, 4, 14, 23, 1423).

Subsequently, the processing proceeds to S104 wherein the n-th node compensating floor reaction force moment Mn_dmd (n=14, 23, 1423) is determined on the basis of the compensating total floor reaction force moment Mdmd determined by the aforesaid posture stabilization control calculator 104. This processing is carried out as described above by the aforesaid compensating total floor reaction force moment distributor 114 a.

Subsequently, the processing proceeds to S106 to determine the vectors V1423, V14 and V23 defined in the explanation of the hierarchical compliance operation and vectors U1423, U14 and U23 orthogonal thereto. In the first embodiment, however, the floor reaction force moments in the directions of the vectors U1423, U14 and U23 cannot be generated; therefore, it is unnecessary to determine U1423, U14 and U23.

Supplementally, as in the second embodiment to be described hereinafter, if the number of child nodes of an n-th node is 3 or more, then Vn may be taken in any direction as long as it does not suddenly change timewise; hence, the direction of Vn may be appropriately determined by, for example, setting it to the direction of the X axis of a supporting leg coordinate system or the direction of the body of a robot. Further, Un is to be orthogonal to Vn.

Subsequently, the processing proceeds to S108 to extract a component Mn_actv in the Vn direction and a component Mn_actu in the Un direction of an actual n-th node floor reaction force moment Mn_act(n=14, 23, 1423). This may be accomplished by carrying out the scalar product calculation of Mn_act and Vn and Un. Incidentally, if the number of child nodes of an arbitrary n-th node that is not a leaf node is two or less, as in the first embodiment, then the component Mn_actu in the Un direction is zero. Hence, it is unnecessary to determine Mn_actu.

Subsequently, the processing proceeds to S110 to extract a component Mn_dmdv in the Vn direction and a component Mn_dmdu in the Un direction of an n-th node compensating floor reaction force moment Mn_dmd(n=14, 23, 1423). This may be accomplished by carrying out the scalar product calculation of Mn_dmd and Vn and Un. Incidentally, if the number of child nodes of an arbitrary n-th node that is not a leaf node is two or less, as in the first embodiment, then the component Mn_dmdu in the Un direction of the n-th node compensating floor reaction force moment Mn_dmd is set to zero. Alternatively, it is unnecessary to determine Mn_dmdu.

Subsequently, the processing proceeds to S112 to determine a Vn component of an n-th node compensating angle θn by multiplying the difference between a value obtained by passing Mn_actv through a filter and a value obtained by passing Mn_dmdv through a filter by a gain Kn (more generally, according to the feedback control law from the difference). In the first embodiment, this processing is carried out as described above on the 14th node, the 23rd node and the 1423rd node by the aforesaid compensating angle determiners 114 b, 114 c and 114 d.

Subsequently, the processing proceeds to S114 to determine a Un component of an n-th node compensating angle θn by multiplying the difference between a value obtained by passing Mn_actu through a filter and a value obtained by passing Mn_dmdu through a filter by the gain Kn (more generally, according to the feedback control law from the difference). However, if the number of child nodes of an arbitrary n-th node that is not a leaf node is two or less, as in the first embodiment, then an n-th node compensating angle U component is set to zero. Alternatively, it is unnecessary to carry out the processing of S114.

The above is the subroutine processing of S34 in FIG. 9. Supplementally, the processing of S106 to S114 may be regarded as the processing for determining an n-th node compensating angle such that the actual n-th node floor reaction force moment acting on a desired n-th node floor reaction force central point converges to an n-th node compensating floor reaction force moment (more precisely, the resultant force of the n-th node compensating floor reaction force moment and the desired node floor reaction force moment acting on the desired n-th node floor reaction force central point).

Subsequently, the processing proceeds to S36 of the flowchart of FIG. 9 to calculate the aforesaid deformation compensation amount. This processing is carried out by the aforesaid deformation compensation amount calculator 114 n as described above.

Subsequently, the processing proceeds to S38 to correct a desired ground contact portion position/posture on the basis of the compensating angle determined in S34 and to further correct it on the basis of the deformation compensation amount determined in S36, thereby obtaining the corrected desired ground contact portion position/posture with deformation compensation of each ground contact portion 10. In the first embodiment, the corrected desired ground contact portion positions of the ground contact portions 10 are determined on the basis of the compensating angles θ1423, θ14 and θ23 as described above (as explained with reference to the aforesaid 15 and FIG. 16) by the corrected desired ground contact portion position/posture calculator 114 g. Then, the determined corrected desired ground contact portion positions are further corrected by the corrected desired ground contact portion position/posture with deformation compensation calculator 114 h on the basis of the aforesaid deformation compensation amount En_cmpn(n=1, 2, 3, 4), as described above, thereby obtaining the corrected desired ground contact portion positions with deformation compensations of the ground contact portions 10.

The processing from S32 to S38 in FIG. 9 explained above is the processing of the hierarchical compliance operation determiner 114.

Subsequently, the processing proceeds to S40 to calculate the joint displacement command of the robot 1 from the desired body position/posture and the corrected ground contact portion position/posture with deformation compensation (the corrected ground contact portion position with a deformation compensation in the first embodiment). This processing is carried out as described above by the aforesaid robot geometric model 110.

Subsequently, the processing proceeds to S42 to make an actual joint displacement follow the joint displacement command. This follow-up control (servo control) is carried out by the aforesaid displacement controller 112.

Subsequently, the processing proceeds to S44 to update time by Δt (the arithmetic processing cycle of the control device 50), and returns to S14 to repeat the processing described above.

Owing to the hierarchical compliance operation as in the first embodiment explained above, the controls of node floor reaction forces hardly interfere with each other, thus allowing the node floor reaction forces to be controlled easily and properly also in a mobile robot having three or more ground contact portions. This restrains control interference and also restrains the actual floor reaction force of each node from deviating from a desirable value or from vibrating. Thus, even if a floor surface not only has an undulation or tilt over a large area thereof but also unexpected changes in the floor configuration, including local projections or depressions or slopes, floor reaction forces acting on a legged mobile robot can be properly controlled without being influenced by them much. Moreover, control for stabilizing the posture of a mobile robot can be easily accomplished and the landing shocks to the mobile robot can be reduced, thus allowing the ground contact properties of the mobile robot to be enhanced and also preventing the mobile robot from slipping or spinning when it travels. Furthermore, the loads of the actuators of the mobile robot can be reduced. Hence, the floor reaction forces of ground contact portions can be properly controlled, so that high posture stability can be obtained.

To supplement the advantages contributing to the stabilization of the posture of a robot, when the entire robot inclines from a state in which the robot is walking as expected on an expected floor surface, the relationship between an inclination angle error θberr (θberrx, θberry) and an increasing amount ΔM of the moment horizontal component about a desired total floor reaction force central point generated in response thereto is preferably a proportional relationship. If not, it is still preferred that expression 24 given below holds for a certain rotational matrix T and a certain diagonal matrix diag(a,b). Incidentally, T, diag(a,b) are secondary square matrixes. T*ΔM=diag(a,b)*T*θberr   Expression 24

If these relationships are not satisfied, then the inclination angle error θberr will not linearly converge when the robot restores its posture from the inclined state, and a precession movement may occur. For instance, in a state wherein the body of a robot inclines forward, the force (restoring force) to fall sideways other than the restoring force for returning backward excessively acts, and the inclination angle error θberr does not linearly returns to zero. Instead, a restoring force acts toward back sideways and the inclination angle error θberr spirally converges to zero.

For the same reason as that described above, in the compliance control, the relationship between a changing rate dθberr/dt of the inclination angle error θberr of the entire robot and an increasing amount ΔM_d of the moment generated in response thereto is also preferably a proportional relationship. If not, it is still preferred that expression 25 given below holds for a certain rotational matrix T and a certain diagonal matrix diag(e,f). Incidentally, T, diag(a,b) are secondary square matrixes. T*ΔM _(—) d=diag(e,f)*T*dθberr/dt   Expression 25

In general, if compliance control is independently carried out on each leg of a robot, these relationships may not be satisfied and a precession movement may take place. In the hierarchical compliance control shown in the first embodiment, the relationships of the aforesaid expressions 24 and 25 can be satisfied, so that the convergence of the posture control of the robot is high and it is possible to prevent oscillation or vibration.

In the aforesaid first embodiment, the desired ground contact portion positions have been amended (corrected) so as to rotationally move about node floor reaction force central points without changing the postures of the ground contact portions (without controlling the postures), thereby correcting the relative heights of the ground contact portions 10 from the relative heights of the desired ground contact portion positions. Alternatively, however, only the heights of the ground contact portions 10 may be corrected by moving the desired ground contact portion positions only in the vertical direction. Specifically, a desired ground contact portion position is corrected according to the following procedure.

First, by the following expressions 26 and 27, a vertical position correction amount Z14 of a desired 14th node floor reaction force central point and a vertical position correction amount Z23 of a desired 23rd node floor reaction force central point are determined. Z14=−PQ14*θ1423   Expression 26 Z23=PQ23*θ 1423   Expression 27

Here, note that the value determined by the aforesaid expression 19 is substituted into θ1423.

Next, the vertical position correction amounts Zn(n=1, 2, 3, 4) of the desired floor reaction force central points of the ground contact portions (the desired node floor reaction force central points of leaf nodes) are determined according to the following expressions. Z1=−Q14Q1*θ14+Z14   Expression 28 Z4=Q14Q4*θ14+Z14   Expression 29 Z2=−Q23Q2*θ23+Z23   Expression 30 Z3=Q23Q3*θ23+Z23   Expression 31

Here, note that the value determined by the aforesaid expression 23 is substituted into θ14, and the value determined in the same manner as that for θ14 is substituted into θ23.

The corrected desired ground contact portion positions are obtained by adding Z1, Z2, Z3 and Z4 determined as described above to the desired ground contact portion positions in the vertical direction.

Further, in the aforesaid first embodiment, the ground contact portions 10 have been hierarchized as shown in the aforesaid FIG. 6; however, the hierarchical structure does not necessarily have to be decided to be one hierarchical structure beforehand. For example, the hierarchical structure may be changed according to the travel mode (the motion mode of legs when traveling) of the robot 1, including trotting or galloping. For instance, the ground contact portions 10 may be hierarchized as shown in FIG. 23. FIG. 23 illustrates, as in the aforesaid FIG. 3(b), an example wherein the pair of the first ground contact portion 10 and the second ground contact portion 10 is defined as a 12th node and the pair of the third ground contact portion 10 and the fourth ground contact portion 10 is defined as a 34th node in a period during which all legs #1 to #4 of the robot 1 are all supporting legs. In this example, the desired node floor reaction force central points Qn(n=1, 2, 3, 4, 12, 34, 1234) of the nodes are set as illustrated. With this arrangement, the aforesaid compliance operation and the estimation of a floor configuration, which will be discussed later, can be performed more accurately in some cases.

Second Embodiment

The following will explain a second embodiment of the present invention with reference to the aforesaid FIG. 1 and FIG. 24 to FIG. 34. Referring to FIG. 1, the explanation will be focused on the aspects of a robot 1 of the second embodiment that are different from the robot of the first embodiment. The robot 1 of the second embodiment is provided with, in addition to the first to the fourth legs #1 to #4, a fifth leg #5 and a sixth leg #6 having the same structures as those of the legs #1 to #4. This means that the robot 1 of the second embodiment is a six-legged robot. The fifth leg #5 is behind the third leg #3 and extended from the right side of the body 24 of the robot 1, and the sixth leg #6 is behind the fourth leg #4 and extended from the left side of the body 24 of the robot 1. The rest of the mechanical structure of the robot 1 is the same as that of the first embodiment, so that the like reference numerals as those of the first embodiment will be assigned and the explanation thereof will be omitted.

The basic concept of the technique of the hierarchical compliance control of the robot 1 (six-legged robot) of the second embodiment is the same as that of the first embodiment. However, the hierarchical compliance control of the second embodiment differs from that of the first embodiment in that there is a node having three child nodes and control processing is expanded to handle them.

The following will explain the control processing of the robot 1 of the second embodiment, focusing mainly on the different aspects. Regarding the reference numerals and terms used in the explanation of the second embodiment, the same reference numerals and terms as those in the first embodiment will be used for those that have the equivalent meanings as those in the first embodiment, and detailed explanation thereof will be omitted.

FIG. 24 is a diagram for explaining the hierarchical structure in the second embodiment, and it is a diagram corresponding to FIG. 3(b) in the first embodiment. As shown in FIG. 24, in the second embodiment, the six individual ground contact portions 10 are defined as leaf nodes (the first to the sixth nodes), and the set of all the six ground contact portions 10 is defined as a root node (the 145236th node). In addition, the set of the first, the fourth, and the fifth ground contact portions 10, which are the ground contact portions of the first leg #1, the fourth leg #4, and the fifth leg #5 is defined as a 145th node, and the set of the second, the third, and the sixth ground contact portions 10, which are the ground contact portions of the second leg #2, the third leg #3, and the sixth leg #6 is defined as a 236th node. In other words, the 145th node is an intermediate node having the first, the fourth, and the fifth nodes (three leaf nodes) as child nodes, and the 236th node is an intermediate node having the second, the third, and the sixth node (three leaf nodes) as child nodes.

In the second embodiment, for example, the set of the three ground contact portions 10 of the 145th node and the set of the three ground contact portions 10 of the 236th node are alternately lifted and landed to move the robot 1. FIG. 24 shows the state wherein both sets are landed (the state wherein both sets are supporting legs). The triangles with the reference marks Qn(n=1, 2, 3, 4, 5, 6) in the figure correspond to the positions of the ground contact portions 10.

Incidentally, Qn(n=1, 2, 3, 4, 5, 6) denotes the desired floor reaction force central point of each of the first to the sixth ground contact portions 10, which are leaf nodes (desired node floor reaction force central points), Q145 and Q236 denote the desired floor reaction force central points of the 145th node and the 236th node, respectively (desired node floor reaction force central points), and P denotes the desired total floor reaction force central point (=desired ZMP) as the desired floor reaction force central point (desired node floor reaction force central point) Q145236 of the root node (the 145236th node). As with the first embodiment, Qn(n=1, 2, 3, 4, 5, 6) agrees with the central point of each corresponding ground contact portion 10.

The overall functional construction of a control device 50 in the second embodiment is the same as that shown in FIG. 2 explained in conjunction with the aforesaid first embodiment.

In this case, a gait generating device 100 in the second embodiment determines and outputs desired motion trajectories (a desired ground contact portion trajectory and a desired body position/posture trajectory) of the robot 1 and desired floor reaction force trajectories (a desired total floor reaction force central point trajectory and a desired total floor reaction force trajectory), as with that in the first embodiment. In the second embodiment, however, the desired ground contact portion trajectory is the trajectory of the desired ground contact portion position of each of the six ground contact portions 10. If the ground contact portions are provided such that their postures are controllable, then the trajectory of desired ground contact portion postures is also included in the desired ground contact portion trajectory. Further, the desired total floor reaction force central point trajectories are determined such that they continuously move while existing in a range in which ZMPs may exist at positions not excessively close to the boundaries of the range (e.g., at an approximately central position of the range wherein a ZMP may exist) according to the motion modes of the legs #1 to #6 (especially the positions at which supporting legs are expected to land) in the second embodiment.

A desired floor reaction force distributor 102 in the second embodiment determines desired node floor reaction force central points, the weights of nodes, and desired node floor reaction forces such that the following conditions A′) to F′) are satisfied, as with the first embodiment.

A′) The desired node floor reaction force central point Qn(n=1, 2, 3, 4, 5, 6) of each leaf node agrees with the central point of the ground contact portion 10 corresponding to the leaf node. More generally, Qn(n=1, 2, 3, 4, 5, 6) is determined according to a desired gait (a desired motion, such as a desired ground contact portion trajectory). For example, if the desired floor reaction force central point of each ground contact portion 10 is determined by the gait generating device 100, then the desired node floor reaction force central point Qn(n=1, 2, 3, 4, 5, 6) may be determined on the basis of the desired floor reaction force determined by the gait generating device 100.

B′) The desired floor node reaction force central point of the root node agrees with a desired total floor reaction force central point P.

C′) The desired node floor reaction force central point Qn of an arbitrary n-th node (n=145, 236, 145236) having child nodes will be a weighted average point of the desired node floor reaction force central points of the child nodes of the n-th node. More specifically, a desired node floor reaction force central point Q145 of a 145th node provides the internally dividing point of a triangle having, as its apexes, desired node floor reaction force central points Q1, Q4 and Q5 of the first, the fourth and the fifth nodes, which are the child nodes (leaf nodes) thereof (points on a boundary of the triangle being included), and a desired node floor reaction force central point Q236 of a 236th node provides the internally dividing point of a triangle having, as its apexes, desired node floor reaction force central points Q2, Q3 and Q6 of the second, the third and the sixth nodes, which are the child nodes (leaf nodes) thereof (points on a boundary of the triangle being included). Further, a desired node floor reaction force central point Q145236 (=desired total floor reaction force central point P) of a 145236th node (root node) provides an internally dividing point of a segment connecting the desired node floor reaction force central points Q145 and Q236 of the 145th node and the 236th node, which are the child nodes (intermediate nodes) thereof.

D′) The desired node floor reaction force Fn of an arbitrary n-th node (n=145, 236, 145236) having child nodes agrees with the sum (resultant force) of the desired node floor reaction forces of all child nodes of the n-th node, and the desired node floor reaction force F145236 of the root node (the 145236th node) agrees with the desired total floor reaction force Ftotalref. Alternatively, the desired node floor reaction force Fn(n=1, 2, 3, 4, 5, 6, 145, 236, 145236) of each node has the relationship of the aforesaid expression 4 a or 4 b with the weight of each node.

E′) The desired node floor reaction force of a leaf node associated with the ground contact portion 10 not in contact with the ground is zero.

F′) The desired node floor reaction force central point, the weight, and the desired node floor reaction force of each node (n-th node (n=1, 2, 3, 4, 145, 236, 145236)) continuously change.

G′) The weight of a leaf node corresponding to the ground contact portion 10 in a no-contact-with-ground state or the weight of any one of the ancestor nodes of the leaf node is zero.

The weight of the root node has been set to “1” for the sake of convenience, as with the aforesaid first embodiment.

The processing of a posture error calculator 103 and a posture stabilization control calculator 104 in the second embodiment is the same as that in the first embodiment; a compensating total floor reaction force moment Mdmd (Mdmdx, Mdmdy) is determined as explained in the first embodiment.

An actual floor reaction force detector 108 in the second embodiment, as that in the first embodiment, detects the actual floor reaction forces acting on the ground contact portions 10 (the six ground contact portions in the present embodiment) from the outputs of six-axis force sensors 34 provided on the legs #1 to #6 and converts them into actual floor reaction forces represented by a supporting leg coordinate system (the global coordinate system shown in FIG. 1, which is fixed to a floor).

Primarily on the basis of the final desired trajectories of ground contact portion positions and desired body position/posture, a robot geometric model (inverse kinematics calculator) 110 in the second embodiment calculates joint displacement commands of the robot 1 that satisfy them by inverse kinematics calculation, as that in the first embodiment.

Further, a displacement controller 112 in the second embodiment controls actuators (not shown) of the joints 14 and 15 of the robot 1 so as to make actual joint displacements follow the aforesaid joint displacement commands, as that in the first embodiment.

Further, a hierarchical compliance operation determiner 114 in the second embodiment corrects the desired ground contact portion trajectories of the ground contact portions 10 so as to satisfy the aforesaid requirements 1) and 2) as much as possible, as that in the first embodiment. The present embodiment, however, has intermediate nodes each having three child nodes, so that the specific processing of the hierarchical compliance operation determiner 114 is somewhat more complicated than that in the first embodiment. If a robot is constructed such that the postures of the ground contact portions 10 are controllable, then the hierarchical compliance operation determiner 114 should correct the desired ground contact portion trajectories of the ground contact portions 10 to also satisfy the aforesaid requirement 3) as much as possible.

FIG. 25 is a block diagram showing the functions of the hierarchical compliance operation determiner 114 of the second embodiment, and it corresponds to FIG. 17 in the first embodiment. Referring to FIG. 25, the hierarchical compliance operation determiner 114 of the second embodiment is equipped with, as its components (functional means), a compensating total floor reaction force moment distributor 114 a, compensating angle determiners 114 b, 114 c and 114 d, a corrected desired ground contact portion position/posture calculator 114 g, a deformation compensation amount calculator 114 n, and a corrected desired ground contact portion position/posture with deformation compensation calculator 114 h, as with the first embodiment. Incidentally, in the second embodiment, compensating angles include a 145236th compensating angle θ145236, a 145th compensating angle θ145, and a 236th compensating angle θ236 related to the 145236th node, the 145th node, and the 236th node, respectively, and the compensating angle determiners 114 b, 114 c and 114 d determines θ145236, θ145, and θ236.

The following will explain the processing of the hierarchical compliance operation determiner 114 in the second embodiment, focusing mainly on the aspects different from the first embodiment.

The hierarchical compliance operation determiner 114 in the second embodiment, as that in the first embodiment, determines the translational force component and the moment component of the desired node floor reaction force of each node primarily on the basis of an output of the desired floor reaction force distributor 102, and also determines the translational force component and the moment component of the actual node floor reaction force of each node on the basis of an output of the actual floor reaction force detector 108.

More specifically, referring to FIG. 26, a translational force component Fn_ref(n=1, 2, 3,4, 5, 6) of the desired node floor reaction force of each leaf node is identical to the translational force component of the desired node floor reaction force determined by the desired floor reaction force distributor 102, and a translational force component F145236 ref of the desired node floor reaction force of the root node is identical to a translational force component Ftotalref of the desired total floor reaction force determined by the gait generating device 100. Further, a translational force component Fn_ref(n=145, 236) of a desired node floor reaction force of an intermediate node other than leaf nodes and the root node is determined to be the translational force component of the resultant force of the desired node floor reaction forces of the child nodes of the intermediate node (F145 ref=F1 ref+F4 ref+F5 ref, F236 ref=F2 ref+F3 ref+F6 ref). In this case, the parent node of the intermediate node is the root node, so that F145 ref+F236 ref=Ftotalref. Incidentally, FIG. 26 illustrates the translational force component Fn_ref of the desired node floor reaction force of each node in a state wherein all the ground contact portions 10 of the robot 1 in the second embodiment are in contact with the ground.

The moment component of the desired node floor reaction force of node (more precisely, the moment component of the desired node floor reaction force having the desired node floor reaction force central point of the node as its point of action) is always set to zero.

Referring to FIG. 30, a translational force component Fn_act(n=1, 2, 3, 4, 5, 6) of the actual node floor reaction force of each leaf node is identical to the translational force component of the actual floor reaction force of each ground contact portion 10 detected by the actual floor reaction force detector 108. Further, a translational force component Fn_act(n=145, 236, 145236) of the actual node floor reaction force of each node having child nodes is determined to be the translational force component of the resultant force of the actual node floor reaction forces of the child nodes of the node. Therefore, F145 act=F1 act+F4 act+F5 act, F236 act=F2 act+F3 act+F6 act, F145236 act (=Ftotalact) =F145 act+F236 act. FIG. 30 illustrates the translational force component Fn_act of the actual node floor reaction force of each node in the state wherein all the ground contact portions 10 of the robot 1 in the second embodiment are in contact with the ground.

Further, referring to FIG. 31, a moment component of the actual node floor reaction force of each leaf node (a moment component having the desired node floor reaction force central point of the leaf node as its point of action) Mn_act(n=1, 2, 3, 4, 5, 6) is basically identical to the moment component of the actual floor reaction force of each ground contact portion 10 detected by the actual floor reaction force detector 108. In the present embodiment, however, no actual moment occurs at the central point of each ground contact portion 10, i.e., the desired floor reaction force central point of the ground contact portion 10; therefore, the moment component of the actual node floor reaction force of each leaf node is set to zero. Further, regarding the moment component Mn_act of the actual node floor reaction force of each n-th node (n=145, 236, 145236) that has child nodes, the resultant force of the actual node floor reaction forces of the child nodes of the n-th node is basically determined to be the moment component (this generally does not become zero) acting on the desired node floor reaction force central point of the n-th node.

The compensating total floor reaction force moment distributor 114 a in the second embodiment distributes the aforesaid compensating total floor reaction force moment Mdmd the aforesaid compensating total floor reaction force moment Mdmd (Mdmdx, Mdmdy) to a 145236th node compensating floor reaction force moment M145236 dmd, a 145th node compensating floor reaction force moment M145 dmd, and a 236th node compensating floor reaction force moment M236 dmd.

The 145236th node compensating floor reaction force moment M1423 dmd is the desired value of the moment to be generated about a desired total floor reaction force central point P (desired ZMP) by the translational force component of the floor reaction force of each ground contact portion 10 generated by manipulating the 145236th compensating angle θ145236 (by rotating the set of the first, the fourth, and the fifth ground contact portions and the set of the second, the third, and the fifth ground contact portions about a desired total floor reaction force central point P(=Q145236) by θ145236).

The 145th node compensating floor reaction force moment M145 dmd is the desired value of the moment to be generated about a desired 145th node floor reaction force central point by the translational force component of the floor reaction force of each ground contact portion 10 (specifically, the first, the fourth, and the fifth ground contact portions) generated by manipulating the 145th compensating angle θ145 (by rotating the first, the fourth, and the fifth ground contact portions 10 belonging to the 145th node about a desired 145th node floor reaction force central point Q145 by θ145).

The 236th node compensating floor reaction force moment M236 dmd is the desired value of the moment to be generated about a desired 236th node floor reaction force central point by the translational force component of the floor reaction force of each ground contact portion 10 (specifically, the second, the third, and the sixth ground contact portions) generated by manipulating the 236th compensating angle θ236 (by rotating the second, the third, and the sixth ground contact portions 10 belonging to the 236th node about a desired 236th node floor reaction force central point Q236 by θ236).

In the present embodiment, as with the first embodiment, the posture of the robot 1 about the vertical axis is not controlled (because the component of the compensating total floor reaction force moment Mdmd about the vertical axis is zero), so that M145236 dmd, M145 dmd, and M236 dmd are all moments (horizontal vectors) whose components about the vertical axis are zero, and the compensating angles θ145236, θ145, and θ236 are rotational angles about the horizontal axis. In particular, M145236 dmd is the moment about the horizontal axis perpendicular to a segment Q145Q236.

These node compensating floor reaction force moments M145236 dmd, M145 dmd, and M236 dmd are basically determined such that the following conditions 11) and 12) are satisfied.

11) On the 145th node, the 236th node, and the 145236th node, as defined in the aforesaid first embodiment, when corrected desired node floor reaction force central points Q145 mdfd, Q236 mdfd, and Pmdfd (=Q145236 mdfd) that satisfy the relational expressions of the following expressions 7 a to 9 a are introduced, these Q145 mdfd, Q236 mdfd, and Pmdfd exist in their respective predetermined existence permissible range. M145236dmd=(Pmdfd−P)*Ftotalref   Expression 7a M145dmd=(Q145mdfd−Q145)*F145ref   Expression 8a M236dmd=(Q236mdfd−Q236)*F236ref   Expression 9a

The existence permissible ranges of Q145 mdfd, Q236 mdfd, and Pmdfd are set, for example, as shown in FIG. 29(a) in a state wherein all the ground contact portions 10 of the robot 1 of the second embodiment are in contact with the ground. More specifically, the existence permissible range of Q145 mdfd is the region on the triangle in the bold line in the figure (the sides of and the region in the triangle), and this is the region set in the triangle having, as its apexes, the desired node floor reaction force central points Q1, Q4, and Q5 of the child nodes of the 145th node such that it is not excessively close to the boundary of the triangle Q1Q4Q4. The existence permissible range of Q236 mdfd is similar to the above. Further, the existence permissible range of Pmdfd is the region on the segment in bold line in the figure, and this is the region set on a segment Q145Q236 connecting the desired floor reaction force central points Q145 and Q236 of the child nodes of the 145236th node (root node) such that it is not excessively close to the end points of the segment Q145Q236.

12) Mdmd=M145236 dmd+M145 dmd+M236 dmd is substantially satisfied.

Further, in the present embodiment, the root node (the 145236th node) has two child nodes, so that M145236 dmd is limited to a vector in the same direction as that of a horizontal unit vector (this being denoted by V145236) orthogonal to the segment Q145Q236, as with each node compensating floor reaction force moment in the aforesaid first embodiment. Hence, in the present embodiment, M145236 dmd, M145 dmd, and M236 dmd are determined such that they satisfy the following condition 13).

13) The component of M145236 dmd+M145 dmd+M236 dmd in the direction of a vector V145236 takes a value close to the component of Mdmd in the direction of the vector V145236 as much as possible.

M145236 dmd, M145 dmd, and M236 dmd satisfying these conditions 11) to 13) are determined, for example, as follows. First, M145236 dmd is determined to be the component of Mdmd in the V145236 direction. However, if the corrected desired node floor reaction force central point Pmdfd determined by the aforesaid 7 a does not fall within its existence permissible range, then M145236 dmd is corrected such that Pmdfd becomes a point of a boundary of the existence permissible range. Subsequently, Mdmd145 and Mdmd236 are determined such that the vector obtained by subtracting M145236 dmd determined as described above from Mdmd substantially agrees with the sum of Mdmd145 and Mdmd236, and the component of Mdmd145+Mdmd23 in the V145236 direction is close to the V145236 component of Mdmd−Mdmd145236 as much as possible and also satisfies the aforesaid condition 11). In this case, Mdmd145 and Mdmd236 are vectors that are parallel to each other.

Examples of M145236 dmd, M145dmd, and M236 dmd determined as described above are shown in FIG. 29(b). Supplementally, M145236 dmd is the horizontal vector perpendicular to the segment Q145Q236 as described above.

If the posture of the robot 1 about the vertical axis is also controlled, the vertical components of M145236 dmd, M145 dmd, and M236 dmd may be also determined.

Compensating angle determiners 114 b to 114 d in the second embodiment basically determine node compensating angles θ145236, θ145, and θ236 on the basis of the difference between the node compensating floor reaction force moment and the actual node floor reaction force moment (the moment having a desired node floor reaction force central point as the point of action) of each n-th node (n=145236, 145, 236) such that the difference is approximated to zero. The examples are shown in FIG. 30 and FIG. 31. In this case, in the present embodiment, θ145 is the angle of rotation in the same direction as the difference M145 act−M145 dmd and about an axis that passes the desired 145th node floor reaction force central point Q145, as shown in FIG. 30, and θ236 is the angle of rotation in the same direction as the difference M236 act−M236 dmd and about an axis that passes the desired 236th node floor reaction force central point Q236, as shown in FIG. 30. Further, θ145236 is the angle of rotation in the same direction as the difference M145236 act−M145236 dmd (the horizontal direction perpendicular to the segment Q145Q236) and about an axis that passes the desired total floor reaction force central point P, as shown in FIG. 31.

However, in the present embodiment also, as with the first embodiment, the node compensating angles θ145236, θ145, and θ236 are determined on the basis of the difference between the node compensating floor reaction force moment of an n-th node (n=145236, 145, 236) that has been passed through a filter and the actual node floor reaction force moment that has been passed through a filter.

FIG. 32 is a block diagram showing the functions of the compensating angle determiner 114 b (θ145236 determiner) that determines the node compensating angle θ145236 as described above, and FIG. 33 is a block diagram showing the functions of the compensating angle determiner 114 c (θ145 determiner) that determines the node compensating angle θ145 as described above. The processing of the compensating angle determiner 114 d (θ236 determiner) is the same as the processing of the θ145 determiner 114 b, so that it will neither be explained nor shown.

The compensating angle determiner (θ145236 determiner) 114 b first determines the moment M145236 act generated about the desired total floor reaction force central point P (the desired 145236th node floor reaction force central point) according to the aforesaid expression 16 or 17 if the translational force component (F1 act+F4 act+F5 act) of the resultant force of actual first, fourth, and fifth node floor reaction forces acts on the desired 145th node floor reaction force central point Q145 and the translational force component (F2 act+F3 act+F6 act) of the resultant force of actual second, third, and sixth node floor reaction forces acts on the desired 236th node floor reaction force central point Q236. Then, a component M145236 actv of the determined M145236 act in the direction of a vector V145236 is determined by scalar product calculation. Here, the vector V145236 is a horizontal unit vector that is perpendicular to the segment Q145Q236. Regarding the 145236th node, it is unnecessary to determine the component of M145236 act in a vector U145236 direction because the positions of the desired floor reaction force central points of the child nodes (the 145th node and the 236th node) cannot be manipulated about the axis in the direction of a horizontal unit vector U145236 perpendicular to V145236.

Subsequently, as with the first embodiment, a difference M145236 errv (=M145236 actvfilt−M145236 dmdvfilt) between M145236 actvfilt obtained by passing the M145236 actv through a low-pass filter and M145236 dmdvfilt obtained by passing a component M145236 dmdv of the aforesaid 145236th node compensating floor reaction force moment M145236 dmd in the direction of a vector V through a compensation filter is multiplied by a predetermined gain K145236 so as to determine the 145236th node compensating angle θ145236.

The compensating angle determiner (θ145 determiner) 114 c first calculates a moment M145 act generated about the 145th node floor reaction force central point Q145 when translational force components F1 act, F4 act, and F5 act of actual first, fourth, and fifth node floor reaction forces act on desired first, fourth, and fifth node floor reaction force central points Q1, Q4, and Q5, respectively. In this case, M145 act to be calculated is formed of components in the directions of vectors V145 and U145, respectively, which are horizontal unit vectors that are orthogonal to each other. The directions of the vectors V145 or U145 may be arbitrary.

Subsequently, a difference M145 err (=M145 actfilt-M145 dmdfilt) between M145 actfilt obtained by passing the M145 act through a low-pass filter and M145 dmdfilt obtained by passing the aforesaid 145th node compensating floor reaction force moment M145 dmd (a component in the vector V direction and a component in the vector U direction) through a compensation filter is multiplied by a predetermined gain matrix K145 (diagonal matrix) so as to determine the 145th node compensating angle θ145. θ145 is constructed of an angle component about the axis of the vector V and an angle component about the axis of the vector U.

The processing for determining the 236th node compensating angle θ236 by the compensating angle determiner (θ236 determiner) 114 d is carried out in the same manner as the processing of the aforesaid θ145 determiner 114 c.

The processing by the compensating angle determiners 114 b to 114 d explained above determines a set of the node compensating angles θ145236, θ145, and θ236 such that the horizontal component of the actual moment Mact acting on the desired total floor reaction force central point P approximates the compensating total floor reaction force moment Mdmd.

As supplementally explained in conjunction with the first embodiment, the node compensating angles θ145236, θ145, and θ236 may be determined as follows. For each n-th node (n=145, 236, 145236) having child nodes, a floor reaction force central point that causes the horizontal component of the moment of the actual node floor reaction force of the n-th node (the resultant force of the actual node floor reaction forces of all child nodes of the n-th node) to be zero is determined as an actual n-th node floor reaction force central point. Alternatively, a floor reaction force central point that causes the horizontal component of the moment, which is obtained by subtracting the moment acting on a desired node floor reaction force central point of each node that is generated by the actual node floor reaction force of the node from the moment of the actual node floor reaction force of an n-th node (n=145, 236, 145236), to be zero is determined as an actual n-th node floor reaction force central point. Then, the difference between the desired node floor reaction force central point of the n-th node (n=145, 236, 145236) and the actual n-th node floor reaction force central point determined as described above, or the difference between the values obtained by passing the node floor reaction force central points through filters, respectively, is defined as a control amount, and the node compensating angles θ145236, θ145, and θ236 are determined on the basis of the control amount (e.g., θ145236, θ145, and θ236 are determined by multiplying the control amount by a certain gain) so as to approximate the control amount to zero.

A corrected desired ground contact portion position/posture calculator 114 g in the second embodiment shown in FIG. 25 corrects the desired ground contact portion position/posture (actually the desired ground contact portion position in the robot shown in FIG. 1) of each ground contact portion 10 so as to obtain a corrected desired ground contact portion position/posture. More specifically, referring to FIG. 30 and FIG. 31, the desired floor reaction force central points Q1, Q4, and Q5 of the first, the fourth, and the fifth nodes, respectively, which are the child nodes of the 145th node, are rotationally moved by the 145th node compensating angle θ145 (horizontal vector), the desired floor reaction force central point Q145 of the 145th node being the center of rotation. The Q1, Q4, and Q5 after the rotational movement are denoted by Q1′, Q4′, and Q5′, respectively. Thus, the 145th node compensating angle θ145 is the manipulated variable for moving the relative relationship among the positions of the desired floor reaction force central points Q1, Q4, and Q4 of the first, the fourth, and the fifth nodes, which are the child nodes of the 145th node, without moving the position of the desired floor reaction force central point Q145 of the 145th node.

Similarly, the desired floor reaction force central points Q2, Q3, and Q6 of the second, the third, and the sixth nodes, respectively, which are the child nodes of the 236th node, are rotationally moved by the 236th node compensating angle θ236 (horizontal vector), about the desired floor reaction force central point Q236 of the 236th node being the center of rotation. The Q2, Q3, and Q6 after the rotational movement are denoted by Q2′, Q3′, and Q6′, respectively. Thus, the 236th node compensating angle θ236 is the manipulated variable for moving the relative relationship among the positions of the desired floor reaction force central points Q2, Q3, and Q6 of the second, the third, and the sixth nodes, which are the child nodes of the 236th node, without moving the position of the desired floor reaction force central point Q236 of the 236th node.

These rotational movements are visually shown in FIG. 30.

Subsequently, the desired floor reaction force central points Q145 and Q236 of the 145th and the 236th nodes, which are the child nodes of the 145236th node, are rotationally moved by the 145236th node compensating angle θ145236 about the axial center in the same direction (horizontal direction orthogonal to a segment Q145Q236) as that of the vector (horizontal vector) of the aforesaid error M145236 errv, the desired floor reaction force central point P (desired total floor reaction force central point) of the 145236th node being the center of rotation. The Q145 and Q236 after the rotational movement are denoted by Q145′ and Q236′, respectively, as shown in FIG. 31. Thus, the 145236th node compensating angle θ145236 is the manipulated variable for moving the relative relationship between the positions of the desired floor reaction force central points Q145 and Q236 of the 145th and the 236th nodes, which are the child nodes of the 145236th node, without moving the position of the desired floor reaction force central point P of the 145236th node (root node).

Next, referring to FIG. 31, the desired node floor reaction force central points Q1′, Q4′, and Q5′ after the previous rotational movement are moved in parallel by a vector Q145_Q145′. This provides final corrected desired node floor reaction force central points Q1″, Q4″, and Q5″ of the first, the fourth, and the fifth nodes. Similarly, the desired node floor reaction force central points Q2′, Q3′, and Q6′ after the previous rotational movement are moved in parallel by a vector Q236_Q236′. This provides final corrected desired node floor reaction force central points Q2″, Q3″, and Q6″ of the second, the third, and the sixth nodes.

Lastly, the desired ground contact portion position of an n-th ground contact portion (n=1, 2, 3, 4, 5, 6) is moved in parallel (substantially in the vertical direction) by a vector Qn_Qn′. This corrects the desired ground contact portion position of each ground contact portion 10 (more precisely, the relative relationship among the desired ground contact portion positions of the ground contact portions 10). More specifically, for each node having child nodes, the manipulated variable (correction amount) of the relative relationship (relative positional relationship) among the desired ground contact portion positions of the ground contact portions 10, which are the descendant nodes of the node, is determined on the basis of the compensating angles θ145, θ236, and θ145236, and combining the manipulated variables (correction amounts) corrects the mutual relative relationship among the desired ground contact portion positions of the ground contact portions 10.

In a robot in which the postures of ground contact portions are controllable and floor reaction force moments can be generated about the desired floor reaction force central points of the ground contact portions, the operation of a foot posture rotation about a desired ground contact portion floor reaction force central point (desired node floor reaction force central point) of each ground contact portion may be performed by the technique shown in Japanese Unexamined Patent Application Publication No. H10-277969 (composite-compliance control). More specifically, as supplementally explained in conjunction with the first embodiment, the desired posture of the n-th ground contact portion may be corrected about Q″ after the n-th ground contact portion is moved in parallel as described above.

A deformation compensation amount calculator 114 n in the second embodiment shown in FIG. 25 determines, as with that in the aforesaid first embodiment, a deformation compensation amount En_cmpn(n=1, 2, 3, 4, 5, 6) for compensating for an influence caused by the deformation of the compliance mechanism 42 of each of the legs #1 to #6. More specifically, the deformation compensation amount calculator 114 n determines a deformation amount En_mdfd(n=1, 2, 3, 4, 5, 6) of the compliance mechanism 42 or the like of the legs #1 to #6 expected to occur due to a corrected desired ground contact portion floor reaction force Fn_refmdfd(n=1, 2, 3, 4, 5, 6) of each ground contact portion 10 (the desired ground contact portion floor reaction force corrected by adding the node compensating floor reaction force moments M145236 dmd, M145 dmd, and M236 dmd to a desired ground contact portion floor reaction force) output from the compensating total floor reaction force moment distributor 114 a in the second embodiment by using a mechanism compliance model. This En_mdfd is multiplied by (−1) to determine the deformation compensation amount En_cmpn.

The corrected desired ground contact portion position/posture with deformation compensation calculator 114 h in the second embodiment shown in FIG. 25 determines, as with that in the first embodiment, the corrected desired ground contact portion position/posture with deformation compensation by adding a corresponding deformation compensation amount En_cmpn to the corrected desired ground contact portion position/posture (the position/posture determined by the aforesaid corrected desired ground contact portion position/posture calculator 114 g) of each ground contact portion 10.

The above is the detailed explanation of the processing of the hierarchical compliance operation determiner 114 in the present embodiment (the second embodiment).

The arithmetic processing of the control device 50 other than that explained above is the same as that of the first embodiment.

Incidentally, the hierarchical structure in the second embodiment may be altered according to an operation mode (traveling mode) or the like of the robot 1, as in the case of the first embodiment. For example, the hierarchical structure may be set as shown in FIG. 34. The example in the figure is equipped with, as intermediate nodes, a 12th node having a first ground contact portion and a second ground contact portion as child nodes (leaf nodes), a 34th node having a third ground contact portion and a fourth ground contact portion as child nodes (leaf nodes), a 56th node having a fifth ground contact portion and a sixth ground contact portion as child nodes (leaf nodes), and a 3456th node having a 56th node and a 34th node as child nodes, the root node having the 3456th node and the 12th node as child nodes. This arrangement makes it possible to perform compliance operations and floor configuration estimations, which will be discussed hereinafter, more accurately in some cases. Incidentally, the meanings of the reference characters in the figure are the same as those shown in the aforesaid FIG. 3(b) or FIG. 24 or the like.

Third Embodiment

An explanation will now be given of a third embodiment in which a floor configuration estimating function and a function for correcting the operation of the robot 1 on the basis of the result of the estimation have been added to the robot 1 (six-legged robot) of the second embodiment. In the present embodiment, the explanation will be focused mainly on the six-legged robot shown in the second embodiment; however, supplemental explanation will be added also to a four-legged robot in some cases. In addition, for the convenience of the understanding of the present embodiment, a two-legged robot will be referred to in some cases.

The mechanical construction of the robot 1 in the present embodiment is the same as that shown in FIG. 1 explained in the first or the second embodiment (except that the robot 1 has six legs #1 to #6). Hence, the explanation of the mechanical construction of the robot 1 will be omitted. The functional construction of a control device 50 provided on the robot 1 in the present embodiment is also the same as that shown in the aforesaid FIG. 2. In the present embodiment, however, the hierarchical compliance operation determiner 114 in FIG. 2 has newly added functions, making it different from that in the second embodiment. Further, the processing of the components of the control device 50 except for the hierarchical compliance operation determiner 114 is the same as that of the second embodiment. Thus, the explanation of the present embodiment will be focused mainly on the processing of the hierarchical compliance operation determiner 114, and a detailed explanation of the processing of the control device 50 other than that will be omitted.

FIG. 35 is a block diagram showing the processing functions of the hierarchical compliance operation determiner 114 in the present embodiment. Of the functions, those functions that are different from those in the second embodiment will be explained. A floor configuration estimator 130 as a floor configuration estimating means and an adder 132 that adds an estimated floor configuration error (more specifically, the estimated value of floor height error, which will be discussed later, related to each ground contact portion 10) output (estimated) by the floor configuration estimator 130 to corrected desired ground contact portion position/posture have been newly added to the hierarchical compliance operation determiner 114 in the present embodiment, and outputs of the adder 132 in place of corrected desired ground contact portion position/posture are supplied to the corrected desired ground contact portion position/posture with deformation compensation calculator 114 h. The rest of the processing of the components of the hierarchical compliance operation determiner 114 is the same as the processing thereof in the second embodiment.

Accordingly, the control processing of the main routine of the control device 50 in the present embodiment is partly different from the control processing shown in the flowchart of the aforesaid FIG. 9. FIG. 36 is a flowchart showing the control processing of the main routine of the control device 50 in the present embodiment. As illustrated, in the present embodiment, the processing for estimating a floor configuration error (the processing of the floor configuration estimator 130) in S37 is newly added after S36. Furthermore, in S38′, desired ground contact portion position/posture are corrected on the basis of the compensating angles θ145236, θ145, and θ236 explained in the aforesaid second embodiment and the floor configuration error estimated in S37, and the desired ground contact portion position/posture after the correction is further corrected on the basis of a deformation compensation amount so as to obtain corrected desired ground contact portion position/posture with deformation compensation, which are final desired ground contact portion position/posture. In this case, more specifically, corrected desired ground contact portion position/posture are determined on the basis of the compensating angles θ145236, θ145, and θ236, as in the second embodiment, then the corrected desired ground contact portion position/posture are corrected on the basis of a floor configuration error, and this is further corrected on the basis of a deformation compensation amount, thereby obtaining corrected desired ground contact portion position/posture with deformation compensation. The items other than the above are the same as the processing of FIG. 9.

Hereinafter, the aspects of the present embodiment that are different from the second embodiment will be specifically explained.

Before starting the detailed explanation of the present embodiment, the concepts and terms to be used when the floor configuration estimator 130 performs estimation will be defined as follows. In the explanation here, for the sake of convenience, schematic diagrams of an average robot, rather than limiting to the six-legged robot 1 in the present embodiment, will be used.

As shown in FIG. 37, FIG. 38, and FIG. 39, a floor (or a floor surface) supposed in a desired gait is referred to as “supposed floor.” An actual floor on which a robot travels is referred to as “actual floor.” For the convenience of explanation, FIG. 37 shows the four-legged robot explained in the first embodiment, and FIG. 38 and FIG. 39 show a two-legged robot. The meanings of the terms explained below remain the same in any multi-legged robots, including the six-legged robot 1 in the present embodiment.

The desired n-th ground contact portion floor reaction force central point Qn defined in the hierarchical compliance control explained in the aforesaid first and the second embodiments has been the point set at the central point of an n-th ground contact portion; however, the floor reaction force central point Qn may alternatively be set on the ground contact surface (bottom surface) of the na-th ground contact portion. In this case, in the desired gait, the point on the supposed floor surface that is supposed to be in contact with the desired n-th ground contact portion floor reaction force central point Qn is referred to as “supposed n-th floor contact point Dn.”

As is obvious from the definition, during the period in which the n-th ground contact portion is in contact with the ground in the desired gait of a robot, the desired n-th ground contact portion floor reaction force central point Qn and the supposed n-th floor contact point Dn share the same coordinates as observed from a supporting leg coordinate system (global coordinate system). In comparison to this, the point at which the point that corresponds to the desired n-th ground contact portion floor reaction force central point Qn on the bottom surface of an actual n-th ground contact portion when the robot 1 is actually traveling comes in contact with an actual floor is referred to as “actual n-th floor contact point Dnact.”

Examples showing the relationships among these points are shown in FIG. 37, FIG. 38, and FIG. 39. Incidentally, FIG. 37 shows a four-legged robot viewed from the direction of the normal line of a vertical plane that passes a desired first ground contact portion floor reaction force central point Q1 and a desired second ground contact portion floor reaction force central point Q2 (that is, substantially sideways), FIG. 38 shows a traveling (walking) two-legged robot viewed from the direction of the normal line of a vertical plane that passes the desired first ground contact portion floor reaction force central point Q1 and the desired second ground contact portion floor reaction force central point Q2 (that is, substantially sideways), and FIG. 39 shows a two-legged robot substantially in an upright posture viewed from the direction of the normal line of a vertical plane that passes the desired first ground contact portion floor reaction force central point Q1 and the desired second ground contact portion floor reaction force central point Q2 (that is, substantially from rear).

In these FIG. 37 to FIG. 39, the sections of the supposed floors on the aforesaid vertical planes are indicated by thin lines, while the sections of actual floors in the aforesaid vertical planes are indicated by thick lines. Incidentally, in FIG. 37, the desired posture of the robot (the entire posture of the robot at an instantaneous value of a desired gait) and the actual posture are indicated by dashed lines and solid lines, respectively. Further, in FIG. 38 and FIG. 39, the desired postures of the robots (the entire postures of the robots at instantaneous values of desired gaits) and actual ground contact portion position/posture are indicated by thin lines and thick lines, respectively. Actual n-th floor contact points in these situations are the points on actual floor surfaces, and they are at the positions shown in FIG. 37, FIG. 38, and FIG. 39.

The configuration difference of an actual floor surface with respect to a supposed floor surface is referred to as a floor configuration error. As the indexes for quantitatively representing floor configuration errors, an n-th node floor height error and an n-th node floor inclination error are defined as follows.

The height of a floor surface at an n-th floor contact point is referred to as “n-th ground contact portion floor height.” In relation to an n-th node that is a leaf, the difference between an actual n-th ground contact portion floor height and a supposed n-th ground contact portion floor height is referred to as “n-th ground contact portion floor height error” or “n-th node floor height error.” The inclination of the floor surface at an n-th floor contact point is referred to as “n-th ground contact portion floor inclination.” In relation to an n-th node that is a leaf, the difference between an actual n-th ground contact portion floor inclination and a supposed n-th ground contact portion floor inclination is referred to as “n-th ground contact portion floor inclination error” or “n-th node floor inclination error.” An example of the ground contact portion floor inclination error is shown in FIG. 39.

With respect to all j-th nodes, which are leaf nodes, the set of node compensating angles for the relationship between desired j-th ground contact portion position/posture and the height and inclination of a supposed j-th floor surface (specifically, the height and inclination of a supposed floor surface at a supposed j-th floor contact point) and the relationship between corrected desired j-th ground contact portion position/posture that has been corrected by a compliance operation using a set of node compensating angles and the height and inclination of an actual j-th floor surface (specifically, the height and inclination of an actual floor surface at an actual j-th floor contact point) to agree with each other is referred to as “(set of) node floor inclination errors),” and among these, the component of the node floor inclination error corresponding to an n-th node compensating angle is referred to as “n-th node floor inclination error.” If an n-th node is a leaf node, then the “n-th node floor inclination error” thus defined agrees with “the n-th node floor inclination error” (=the difference between an actual n-th ground contact portion floor inclination and a supposed n-th ground contact portion floor inclination) defined in relation to leaf nodes previously described.

After all, the set of node floor inclination errors corresponds to a compensation amount necessary to shift all ground contact portions of a robot that is traveling according to a desired gait from a state in which they are parallel to a desired floor surface to a state in which they are parallel to an actual floor surface.

Therefore, by estimating a floor configuration error while walking and by adding the estimated floor configuration error to desired ground contact portion position/posture, the actual floor reaction force moment of each node will be the same as that when the robot is walking on a supposed floor even if there is a floor configuration error. Naturally, the actual floor reaction force moment of the ground contact portion, which is the actual floor reaction force moment of a leaf node, agrees with a desired ground contact portion floor reaction force moment.

Normally, a floor configuration may be represented using the “n-th node floor inclination error” defined as above. If, however, a certain node has three child nodes, and the desired floor reaction force central points of the three child nodes are aligned on the same straight line or if a certain node has four or more child nodes, then it would be difficult to represent a floor configuration.

Hence, in the following explanation, the following expressions that can be generally applied will be used for n-th nodes that are not leaves. The weights used to determine a predetermined weighted average in the following definition are to be the same as the aforesaid weights determined as described above by the desired floor reaction force distributor 102.

Definition:

When all the heights and inclinations of a desired ground contact portion are set to agree with the corresponding heights and inclinations of an actual floor surface, and when the desired floor reaction force central point of an arbitrary node is expressed by a predetermined weighted average of the desired floor reaction force central points of all child nodes thereof (i.e., an internally dividing point based on a predetermined internal division ratio), on an arbitrary n-th node, the height obtained by subtracting the height (the position in the vertical direction) of the desired floor reaction force central point of a parent node of the n-th node from the height (the position in the vertical direction) of the desired floor reaction force central point of the n-th node is referred to as “actual n-th node relative floor height.”

Definition:

When all the heights and inclinations of a desired ground contact portion are set to agree with the corresponding heights and inclinations of a supposed floor surface, and when the desired floor reaction force central point of an arbitrary node is expressed by a predetermined weighted average of the desired floor reaction force central points of all child nodes thereof (i.e., an internally dividing point based on a predetermined internal division ratio), regarding an arbitrary n-th node, the height obtained by subtracting the height (the position in the vertical direction) of the desired floor reaction force central point of a parent node of the n-th node from the height (the position in the vertical direction) of the desired floor reaction force central point of the n-th node is referred to as “supposed n-th node relative floor height.”

The height obtained by subtracting a supposed n-th node relative floor height from an actual n-th node relative floor height is referred to as an “n-th node relative floor height error.” The n-th node relative floor height error will take the same value when defined as follows.

Definition:

When all the heights and postures of desired ground contact portions are made to agree with corresponding ground contact portion floor height errors and ground contact portion floor inclination errors, and the desired floor reaction force central point of an arbitrary node is expressed by a predetermined weighted average of the desired floor reaction force central points of all child nodes thereof (i.e., an internally dividing point based on a predetermined internal division ratio), on an arbitrary n-th node, the height obtained by subtracting the desired floor reaction force central point of a parent node of the n-th node from the desired floor reaction force central point height of the n-th node is referred to as “n-th node relative floor height error.”

As is obvious from the definition of the n-th node relative floor height error described above, regarding each node having child nodes, an n-th node relative floor height error of all child nodes thereof represents the relative relationship of floor height errors. Further, regarding each node having child nodes, the weighted average value of all child nodes thereof is zero.

In the present embodiment (the third embodiment), the difference between an actual floor surface and a supposed floor surface on a desired gait (i.e., a floor configuration error) is compensated so as to correct the desired positions/postures of ground contact portions 10 to make the ground contact portions 10 that are to be in contact with the ground properly come in contact with the actual floor surface. Further, in the present embodiment, the n-th node relative floor height error defined as described above is used as a floor configuration parameter representing a floor configuration error, and based on this, each n-th ground contact portion floor height error (floor configuration error) is estimated. Then, based on each estimated n-th ground contact portion floor height error (hereinafter referred to simply as the n-th floor height error in some cases), the desired position of the n-th ground contact portion 10 is corrected.

Based on the above premises, essential sections of the present embodiment will be explained in detail.

The inputs into the floor configuration estimator 130 of the hierarchical compliance operation determiner 114 shown in FIG. 35 are, in general, roughly divided as follows.

-   1) Desired floor reaction force central points (desired total floor     reaction force central points P and desired n-th ground contact     portion floor reaction force central points Qn(n=1, 2, 3, 4, 5, 6)). -   2) Supposed floor surface configuration (coordinates of supposed     n-th floor contact points and supposed n-th ground contact portion     floor inclinations). -   3) Final desired postures to be followed by an actual robot     (corrected desired ground contact portion positions/postures with     deformations) or actual joint displacements or frequency weighted     average thereof (weighted average based on a weight having a     frequency characteristic). -   4) Body posture inclination errors. -   5) Actual floor reaction forces (the translational force component     and the moment component of an actual n-th ground contact portion     floor reaction force).

Incidentally, in the robot 1 in the present embodiment, the postures of the ground contact portions 10 cannot be controlled, so that the supposed n-th ground contact portion inclination of a supposed floor surface configuration is unnecessary.

Further, in the robot 1 in the present embodiment, the moment component of an actual n-th ground contact portion floor reaction force of an actual floor reaction force is zero. This, therefore, is not required to be supplied to the floor configuration estimator 130. However, in the explanation of the present embodiment hereinafter, the n-th ground contact portion floor reaction force generally includes the moment component of an actual n-th ground contact portion floor reaction force also, considering a case where the postures of the ground contact portions 10 are controllable. Further, in the following explanation, the total number of ground contact portions will be frequently referred to as “last leaf node number” so as to make it possible to expandably apply the present embodiment easily also to a case where the number of ground contact portions is other than four or six. For example, in the six-legged robot 1 shown in FIG. 1, the last leaf node number is “6”.

FIG. 40 is a block diagram showing the processing functions of the floor configuration estimator 130. The functional elements constituting the floor configuration estimator 130 shown in FIG. 40 will be explained. The floor configuration estimator 130 is equipped with a mechanism compliance model 134.

The mechanism compliance model 134 determines the deformation amount of a compliance mechanism 42 or the like of each of the legs #1 to #6 (the compliance mechanism 42 and a link mechanism of each leg) when each of the ground contact portions 10 is subjected to an actual floor reaction force Fn_act(n=1, 2, . . . , last leaf node number), and adds the determined deformation amount to the corrected desired ground contact portion position/posture with deformation compensation, which are supplied to the aforesaid robot geometric model 110 (refer to FIG. 2), so as to determine estimated n-th ground contact portion position/posture (n=1, 2, . . . , last leaf node number), which are the estimated values of the position/posture of an n-th ground contact portion (each ground contact portion) after mechanism deformation (more specifically, the estimated values of the n-th ground contact portion position/posture when it is assumed that an actual body posture agrees with a desired body posture). Incidentally, the corrected desired ground contact portion position/posture with deformation compensation used in this case are the past values of the values or the like determined at the last control cycle.

When determining the estimated ground contact portion position/posture after mechanism deformation, if a follow-up delay of control of a joint displacement of the robot 1 cannot be ignored, then the position/posture obtained by passing corrected desired ground contact portion position/posture with deformation compensation through a low-pass filter corresponding to the follow-up delay may be used instead of using the corrected desired ground contact portion position/posture with deformation compensation as it is.

Alternatively, from a detected value of an actual joint displacement of the robot 1, an actual ground contact portion position/posture without mechanism deformation, which is an actual ground contact portion position/posture in a case where there is no mechanism deformation, are determined through a robot geometric model (the same model as the robot geometric model 110 in FIG. 2), and the determined values may be used in place of the corrected desired ground contact portion position/posture with deformation compensation.

Alternatively, a weighted average of the aforesaid actual ground contact portion position/posture without mechanism deformation and the corrected desired ground contact portion position/posture with deformation compensation may be determined by using a frequency weight (weight having a frequency characteristic), and this may be used in place of the corrected desired ground contact portion position/posture with deformation compensation.

Strictly speaking, the actual ground contact portion position/posture without mechanism deformation mentioned above should be used; however, any one of the above may be used without much difference as long as the capability of joint displacement control is high.

Incidentally, the mechanism compliance model 134 is explained in detail in Japanese Unexamined Patent Application Publication No. H10-277969 previously proposed by the present applicant; therefore, no more explanation will be given herein.

Supplementally, in a case where an input to the robot geometric model 110 in FIG. 2 does not include deformation compensation, that is, if corrected desired ground contact portion position/posture without deformation compensation (the position/posture determined by the aforesaid corrected desired ground contact portion position/posture calculator 114 g) is supplied to the robot geometric model 110, then the corrected desired ground contact portion position/posture without deformation compensation may be supplied to the mechanism compliance model 134 of the floor configuration estimator 130.

Returning to the explanation of FIG. 40, the estimated ground contact portion position/posture after mechanism deformation (n=1, 2, . . . , 6) is then rotationally moved by the aforesaid body posture inclination error θberr, about the desired total floor reaction force central point P being the center of the rotation, thereby to determine n-th estimated ground contact portion position/posture after mechanism deformation (n=1, 2, . . . , last leaf node number) observed from a global coordinate system (supporting leg coordinate system). Subsequently, the position of the point corresponding to the desired n-th ground contact portion floor reaction force central point after the mechanism deformation observed from the global coordinate system is determined on the basis of the n-th estimated ground contact portion position/posture after the mechanism deformation (n=1,2, . . . , last leaf node number) observed from the global coordinate system and the position of the desired n-th ground contact portion floor reaction force central point observed from the desired ground contact portion position of an n-th ground contact portion (the desired position of the representative point of the n-th ground contact portion). Then, the determined position of the point is defined as an instantaneous estimated floor contact point position (instantaneous estimated n-th floor contact point position) Qn_estm′. This Qn_estm′ corresponds to the estimated value of the instantaneous position of an actual n-th floor contact point. The method for determining the instantaneous estimated floor contact point position Qn_estm′ are equivalent to determining Q″ of expression 21 in Japanese Unexamined Patent Application Publication No. H10-277969, so that no further detailed explanation thereof will be given herein.

Subsequently, the result obtained by subtracting the supposed n-th floor contact point position Dn from the instantaneous estimated n-th floor contact point position Qn-estm′ provides a bias-included instantaneous n-th ground contact portion floor height error. This Zfn_with_bias′ corresponds to the instantaneous estimated value of an n-th ground contact portion floor height error. In a case where no actual height of a body is detected, as with the present embodiment, a bias error is usually included. Hence, Zfn_with_bias′ is referred to as the bias-included instantaneous n-th ground contact portion floor height error.

Here, as previously defined, the supposed n-th floor contact point Dn lies at the same position as a desired n-th ground contact portion floor reaction force central point Qn in a period during which an n-th ground contact portion is in contact with the ground. In a desired gait, immediately before an n-th ground contact portion comes in contact with the ground, the position of the desired n-th ground contact portion floor reaction force central point Qn that is expected at the next ground contact is defined as the supposed n-th floor contact point Dn. In the desired gait, immediately after the n-th ground contact portion leaves a floor, the position of the desired n-th ground contact portion floor reaction force central point Qn that was expected at the time of leaving the floor is defined as the supposed n-th floor contact point Dn.

Subsequently, an estimated n-th node floor height error Zfn_estm (n=1, 2, . . . , last leaf node number) is determined by the floor height error estimation processing subroutine of the flowchart of FIG. 41 on the basis of the bias-included instantaneous n-th ground contact portion floor height error Zfn_with_bias′ (n=1, 2, . . . , last leaf node number) and the n-th ground contact portion floor reaction force Fn_act (n=1, 2, . . . , last leaf node number).

Here, “hierarchical relativization processing” (or “hierarchical relativization”) used in the floor height error estimation processing subroutine or the like will be explained.

The hierarchical relativization processing is generally defined as the processing for determining the output values of all nodes relative to the sets of input values (the values of predetermined types of state amounts) to all leaf nodes. More specifically, the hierarchization relativity processing is the processing for determining node output values such that the weighted average of output values corresponding to all childe nodes of an arbitrary node that is not a leaf node is zero and the input value (state amount) of an arbitrary leaf node agrees with the sum of the output value of the node and the output values of all ancestor nodes of the node.

Hereinafter, determining the value of a certain type of output BB by the hierarchical relativization processing from a certain type of input (state amount) AA to a leaf node will be referred to as “determining BB by hierarchically relativizing AA.”

The following will explain the algorithm of the hierarchical relativization processing. In general, an input of the hierarchical relativization processing is an n-th ground contact portion height Zfn(n denoting a leaf node number) and an output thereof is an n-th node relative height Zn_re1(n=1, 2, . . . , last node number). Incidentally, the “last node number” means the largest number among all node numbers, and the last node number is, for example, 145236 in the hierarchical structure explained in the second embodiment. Further, in the explanation of the algorithm, the “n-th ground contact portion height” is a designation that generically represents an input of the hierarchical relativization processing, such as the aforesaid n-th ground contact portion floor height or an n-th ground contact portion floor height error, and the “n-th node relative height” is a designation that generically represents an output of the hierarchical relativization processing, such as an n-th ground contact portion relative floor height error, which will be discussed later.

First, a bias-included n-th node height Zn_with_bias is determined as follows. If an n-th node is a leaf node (i.e., the ground contact portion), then the value of an n-th ground contact portion height Zfn is substituted into the bias-included n-th node height Zn_with_bias. If the n-th node is not a leaf node (i.e., the ground contact portion), then the weighted average of the bias-included n-th node heights Zn_with_bias of all child nodes of the n-th node is determined. The determined weighted average is defined as the bias-included n-th node height Zn_with_bias. It should be noted that a weight Wj(j=1,2, . . . ) determined by the aforesaid desired floor reaction force distributor 102 is used as the weight for each child node.

More specifically, the bias-included n-th node height Zn_with_bias is obtained by expression 32 given below. If the n-th node is a leaf node;  Zn_with_bias=Zfn If the n-th node is not a leaf node;  Zn_with_bias=Σ(Zj_with_bias*Wj)  where Σ denotes the total sum on j, which is j∈{set of child node numbers of the n-th node}    ... Expression 32

According to the above rules, the bias-included node height Zn_with_bias (n=1, 2, . . . , last node number) is determined on every node.

Lastly, an n-th node relative height Zn_re1 is determined by subtracting the bias-included height Zh_with_bias of a parent node of the n-th node (this is supposed to be an h-th node) from the bias-included n-th node height Zn_with_bias.

More specifically, the n-th node relative height Zn_re1 is determined according to the following expression 33. It should be noted that the h-th node is the parent node of the n-th node. Zn _(—) re1=Zn_with_bias−Zh_with_bias   Expression 33

According to the above rule, the node relative floor height Zn_re1(n denoting the number of each node) is determined on every node. It should be noted that a node relative height Zk_re1(k denoting the number of a root node) with respect to a root node is zero. FIG. 42 shows an example of calculation of a node relative height of a four-legged robot shown in the first embodiment, while FIG. 43 shows an example of calculation of a node relative height of the six-legged robot in the present embodiment.

The bias-included n-th node height Zn_re1_with_bias may alternatively be determined by the following method, which provides the same results.

If an n-th node is a leaf node (i.e., the ground contact portion), then the value of an n-th ground contact portion height Zfn is substituted into the bias-included n-th node height Zn_with_bias. If the n-th node is not a leaf node (i.e., the ground contact portion), then the weighted average of the bias-included heights of the leaf nodes of all descendants of the n-th node is determined. The determined weighted average is defined as the bias-included n-th node height Zn_with_bias. It should be noted that a weight Wj′ is the product of the weight Wj determined for the node j by the aforesaid desired floor reaction force distributor 102 and the weight determined by the aforesaid desired floor reaction force distributor 102 for an ancestor node of the node j and all nodes, which are the descendant nodes of the n-th node.

More specifically, the bias-included n-th node height Zn_with_bias is obtained by expression 34 given below. If the n-th node is a leaf node;  Zn_with_bias=Zfn If the n-th node is not a leaf node;  Zn_with_bias=Σ(Zj_with_bias*Wj’)  where Σ denotes the total sum on j, which is j∈{set of leaf node numbers of the descendants of a node n}    ... Expression 34

Supplementally, the weight Wj′ in this case agrees with the value obtained by dividing the desired floor reaction force of each leaf node by a desired j-th node floor reaction force if the desired j-th node floor reaction force is not zero.

The above is the hierarchical relativization processing.

The subroutine processing of the estimation processing of a floor height error will now be explained below by mainly using the flowchart of FIG. 41 showing it. Incidentally, this processing uses an n-th ground contact portion floor height error as an input of the hierarchical relativization processing, and an n-th node relative floor height error as an output. The like symbols as those shown in the aforesaid expressions 32 to 34 or the like will be used as the reference symbols of these input and output.

Referring to FIG. 41, first, in S50, the aforesaid bias-included instantaneous n-th ground contact portion floor height error Zfn_with_bias′ is hierarchically relativized to determine an instantaneous n-th node relative floor height error Zn_rel′ (n=1, 2, . . . , last node number).

Subsequently, in S52, the output obtained by hierarchically relativizing an estimated n-th ground contact portion floor height error, which is an estimated value of the n-th ground contact portion floor height error determined at the last control cycle of the control device 50 (hereinafter referred to as the last estimated n-th ground contact portion floor height error Zfn_estm_p), is determined as the last estimated n-th node relative floor height error Zn_rel_estm_p(n=1, 2, . . . , last node number) . It should be noted that, as the weight of each node used in this hierarchical relativization processing, the weight Wn(n=1, 2, . . . last node number) determined by the aforesaid desired floor reaction force distributor 102 at the current (present) control cycle of the control device 50 is used.

Supplementally, in general, if the weight determined in the current control cycle (hereinafter referred to as the current weight) and the weight determined in the last control cycle (hereinafter referred to as the last weight) are different, then the value of the last estimated relative floor height error of each node determined using the last weight and the value of the last estimated relative floor height error of each node determined using the current weight will be inconveniently different values even if the configuration of an actual floor does not change. Hence, the last estimated node relative floor height error of each node is calculated anew by using the current weight as described above instead of directly using the estimated relative floor height error of each node determined at the last control cycle as the last estimated node relative floor height error of each node.

Subsequently, in S54, an n-th node relative floor height error correction amount candidate value Zn_inc_cand(n=1, 2, . . . , last node number) is determined according to expression 35 given below. The Zn_inc_cand means the provisional value of the correction amount of the error when the n-th node relative floor height error is updated at the current control cycle.

In this case, the n-th node relative floor height error correction amount candidate value Zn_inc_cand is set to the product of the difference between the instantaneous n-th node relative floor height error Zn_rel′ and the last estimated n-th node relative floor height error Zn_rel_estm_p and a predetermined coefficient (ΔT/(Testm+ΔT)), as shown by expression 35 given below. Zn _(—) inc _(—) cand=(Zn _(—) rel′−Zn _(—) rel _(—) estm _(—) p)*ΔT/(Testm+ΔT)   Expression 35

where Testm in expression 35 denotes a time constant (the time constant of a primary delay) at the estimation (update) of an n-th node floor height error, and ΔT is a control cycle of the control device 50.

Thus, when Zn_inc_cand is set, Zn_inc_cand is added to Zn_rel_estm_p for each control cycle of the control device 50, causing the value of the result of the addition to change so as to gradually approximate to Zn_rel′.

Subsequently, in S56, a node request mode (an n-th request mode mdn_dmd), which is a required value of the mode of each node, is determined on the basis of a timing of a desired gait.

Specifically, in the aforesaid six-legged robot 1, an n-th node request mode (n=1, 2, 3, 4, 5, 6), a 145th node request mode md145 dmd, a 236th node request mode md236 dmd, and a 145236th node request mode md145236 dmd are set as shown in the timing chart of FIG. 44 with respect to the elapse of time of a desired gait, and a current request mode is determined on the basis thereof. However, if the gait is different, then the pattern of a node request mode should be changed accordingly. Incidentally, regarding ON/OFF in the timing charts of a first stage (the uppermost stage) and a second stage in FIG. 44, the state in which the ground contact portion is in contact with the ground corresponds to ON, while the state in which it is not in contact with the ground corresponds to OFF.

Supplementally, as the present applicant has disclosed in Japanese Unexamined Patent Application Publication No. H10-277969, in the case of a robot that has feet as ground contact portions and that is capable of estimating floor inclination errors at the ground contact portions, the request mode corresponding to the floor inclination error estimation of the ground contact portions should be set as explained in the Publication.

As shown in FIG. 44, the mode of each node (hereinafter referred to simply as the node mode) comes in ready mode, hold mode, and reset mode.

The ready mode is the mode for estimating a floor configuration. The hold mode is the mode for holding (for maintaining an immediately preceding value) the estimated value of the floor configuration (an estimated n-th ground contact portion floor height error). The hold mode is made to exist in a period during which the estimated value of the floor configuration may diverge and a period during which the accuracy of the estimated value of the floor configuration may deteriorate. Further, the reset mode is the mode for shifting the estimated value of the floor configuration to a predetermined initial value before the next floor configuration estimation is started (the next ready mode begins).

Regarding an n-th node (n=1, 2, 3, 4, 5, 6), which is a leaf node, if an n-th ground contact portion is in contact with the ground on a desired gait (that is, if a desired n-th ground contact portion floor reaction force is not zero), then the n-th node request mode is set to the ready mode. Thereafter, when the n-th ground contact portion leaves a floor on the desired gait (that is, when the desired n-th ground contact portion floor reaction force becomes zero), the n-th node request mode is set to the hold mode immediately thereafter. Then, after a while, the n-th node request mode is set to the reset mode. Further, immediately before the n-th ground contact portion comes in contact with the ground on the desired gait, the n-th node request mode is set to the ready mode.

Regarding an n-th node (n=145, 236, 145236), which is not a leaf node, that is, an n-th node having child nodes, if at least one ground contact portion belonging to the n-th node is in contact with the ground on the desired gait, then the n-th node request mode is set to the ready mode. Thereafter, when all ground contact portions belonging to the n-th node leave the floor on the desired gait, then the n-th node request mode is set to hold until immediately thereafter. Alternatively, the n-th node request mode may be set to the hold mode from immediately before all the ground contact portions belonging to the n-th node leave the floor. Then, after a while, the n-th node request mode is set to the reset mode. Further, immediately before at least one of the ground contact portions belonging to the n-th node comes in contact with the ground on the desired gait, the n-th node request mode is set to the ready mode.

Subsequently, in S58, it is determined whether an estimation enable condition is satisfied, and the mode of each node is finally determined on the basis of the result of the determination and the node request mode. The mode of each node to be determined is one of the aforesaid ready mode, hold mode, and reset node. The aforesaid node request mode has been determined on the basis of whether the ground contact portions are in contact with the ground or not on the desired gait. In S58, the mode of each node is determined, considering whether the ground contact portions are actually in contact with the ground or not.

The estimation enable condition is satisfying either expression 36 or 37 given below. In this case, if neither expression 36 or 37 is satisfied, then the estimation enable condition does not hold. Fn_act_z>Fn_min   Expression 36 Zn_rel′<Zn_rel_estm_p   Expression 37

where Fn_act_z denotes the translational force vertical component of an actual n-th ground contact portion floor reaction force (n=1, 2, . . . , last leaf node number).

The estimation enable condition is a condition that when an n-th node relative floor height error is estimated (when a floor configuration error is estimated), the estimated value does not diverge. The divergence here means a situation in which, when a correcting operation is performed to modify corrected desired ground contact portion position/posture so as to offset the influence of an actual n-th floor height error by using an estimated n-th floor height error Zfn_estm (the estimated value of the floor configuration error) that has been estimated as it will be discussed later, the estimated n-th floor height error Zfn_estm continues to increase and an n-th ground contact portion moves away from the floor (floats further from the floor).

If the estimation of Zfn_estm is ideally executed, a predetermined permissible value Fn_min may be zero, whereas in actuality, a detection error of Fn_act takes place, causing an estimated value to diverge in some cases. To prevent it, Fn min is set to be a value that is sufficiently larger than a detection error of Fn_act.

In the present embodiment, the estimation enable condition is the condition in which divergence described above does not occur. However, the estimation enable condition may alternatively be a condition in which a predetermined accuracy of estimating a floor configuration can be secured. Hence, in place of the translational force vertical component of the actual n-th ground contact portion floor reaction force Fn_act, a component that is perpendicular to a supposed floor surface (or an estimated floor surface (estimated actual floor surface)) out of the translational force components of the actual n-th ground contact portion floor reaction force Fn_act may be used.

Incidentally, if a floor configuration error (a configuration error between an actual floor surface and a supposed floor surface) is simply estimated and not reflected in a walking operation, that is, if the correction of the corrected desired ground contact portion position/posture to cancel the influence of a floor configuration error is not carried out by using an estimated value of the floor configuration error, then the estimated value of the floor configuration error does not diverge.

The mode of each node is determined on the basis of the result of the determination on whether the estimation enable condition is satisfied and a node request mode.

If an n-th node request mode is the ready mode and the estimation enable condition is satisfied, then the n-th node mode is set to the ready mode.

If an n-th node request mode is the ready mode and the estimation enable condition is not satisfied, then the n-th node mode is set to the hold mode.

If an n-th node request mode is the hold mode, then the n-th node mode is set to the hold mode.

If an n-th node request mode is the reset mode, then the n-th node mode is set to the reset mode.

Thus, an n-th mode is determined to be the ready mode at the end only if the request mode is the ready mode and the estimation enable condition is satisfied.

Subsequently, in S60 to S70, the n-th node relative floor height error (n=1, 2, . . . , last node number) associated with each of all nodes is estimated.

Supplementally, as disclosed in Japanese Unexamined Patent Application Publication No. H10-277969, in the case of a robot that has ground contact portions (feet) whose postures can be controlled and that is capable of estimating the floor inclination errors in the ground contact portions, it is preferred to also estimate the floor inclination errors associated with the ground contact portions.

Therefore, the processing of S60 to S70 is configured so as to allow floor configuration errors to be estimated also when the postures of ground contact portions can be controlled.

The following will specifically explain the processing of S60 to S70. In S62, the quantity of child nodes of an n-th node is determined. At this time, if the number of child nodes (the quantity of child nodes) is two, then in S64, the floor configuration estimation processing for two child nodes, which is the estimation processing of the floor configuration error for the quantity, is carried out. If the number of child nodes is three, then in S66, the floor configuration estimation processing for three child nodes, which is the estimation processing of the floor configuration error for the quantity, is carried out. If the number of child nodes is zero, then it is determined in S68 whether the n-th node ground contact portion permits control of a floor reaction force moment.

The processing for the case where the number of child nodes is zero will be further explained. If the number of child nodes of an n-th node is zero, then the n-th node is a leaf node, and the n-th node corresponds to the ground contact portion. In this case, as described above, first, it is determined in S68 whether the n-th ground contact portion permits control of a floor reaction force moment. Here, for example, in the case of a robot that has feet whose postures can be controlled as ground contact portions and its legs do not include free joints, as with a regular bipedal walking robot, floor reaction force moments can be generated at the ground contact portions. However, in the robot 1 (six-legged robot) in the present embodiment, the ground contact portions 10 engage the spherical joints 12, which are free joints; therefore, none of the ground contact portions 10 can generate floor reaction force moments. In this case, no processing is carried out on the n-th node whose number of child nodes is zero.

Meanwhile, if it is determined in S68 that floor reaction force moments can be controlled, then ground contact portion floor inclination error estimation processing is carried out in S70. This processing is the processing for estimating the floor inclination error in each ground contact portion. This processing is the processing in which the foot floor reaction force central point in the floor inclination estimation processing in Japanese Unexamined Patent Application Publication No. H10-277969 previously proposed by the present applicant has been replaced by the desired floor reaction force central point of an n-th ground contact portion in the present embodiment. Hence, further detailed explanation will be omitted in the present description.

Next, the aforesaid floor configuration estimation processing for two child nodes will be explained with reference to FIG. 45 to FIG. 48. FIG. 45 is a flowchart showing the subroutine processing of the floor configuration estimation processing for two child nodes, FIG. 46 and FIG. 47 are flowcharts showing the subroutine processing of S6404 and S6408, respectively, of FIG. 45, and FIG. 48 is a flowchart showing the subroutine processing of S6406 and S6410 of FIG. 45.

In the floor configuration estimation processing for two child nodes, the modes of all child nodes of an n-th node having two child nodes (the mode determined in S58 of FIG. 41) are determined in S6400, as shown by the flowchart of FIG. 45. The results of the determination are divided into three, namely, “all ready,” “all reset,” and “others (else).” In the following explanation, the two child nodes of the n-th node will be denoted as the i-th node and the j-th node.

Here, if it is determined that the mode of the child nodes is “all ready,” then it is determined in S6402 whether a resultant force Fn_z (=Fi_act_z+Fj_act_z) of the translational force vertical components Fi_act_z, Fj_act_z of the actual node floor reaction forces of the child nodes of the n-th node is larger than a predetermined value Fn_min2. In other words, Fn_z denotes the translational force vertical component of the resultant force of the actual floor reaction forces of all ground contact portions belonging to the n-th node.

If the determination result of S6402 is YES, then intra-group complete estimation processing for a node having two child nodes (the processing for virtually estimating the node relative floor height error of each of the two child nodes owned by the n-th node) is carried out in S6404. In this processing, estimated node relative floor height errors Zi_rel_estm and Zj_rel_estm of the i-th node and the j-th node, respectively, which are the two child nodes of the n-th node, are determined (updated) according to the expressions shown in the flowchart of FIG. 46. More specifically, Zi_rel_estm will be representatively explained. A j-th node relative floor height error correction amount candidate value Zi_inc_cand determined in the aforesaid S54 is added to a value Zi_rel_estm_p of Zi_rel_estm in the last control cyley thereby to determine a new estimated j-th node relative floor height error Zi_rel_estm. The same applies to the j-th node.

Further, if the resultant force Fn_n of the floor reaction forces of the two child nodes of the n-th node is smaller than the predetermined value Fn_min2 (if the determination result of S6402 is NO), then it means that the accuracy of estimating a floor configuration error would be excessively deteriorated, so that no substantial estimation processing is carried out, and the intra-group total hold processing for a node having two child nodes (the processing for holding the estimated node relative floor height errors of the two child nodes of the n-th node without updating them) is carried out in S6406. In the hold processing, as shown by the expressions in the flowchart of FIG. 48, the values of the estimated node relative floor height errors Zi_rel_estm and Zi_rel_estm of the i-th node and the j-th node, respectively, are maintained at the values Zi_rel_estm_p and Zi_rel_estm p at the last control cycle.

Next, if it is determined in S6400 of FIG. 45 that the modes of the two child nodes are “all reset,” then intra-group total reset processing for a node having two child nodes (the processing for resetting the estimated node relative floor height error of each of the two child nodes owned by the n-th node) is carried out in S6408. In the reset processing, the estimated node relative floor height errors Zi_rel_estm and Zi_rel_estm of the i-th node and the j-th node, respectively, are updated according to the expressions in the flowchart of FIG. 47 such that they gradually approximate to zero. Incidentally, the meanings of ΔT and Testm in the expressions are the same as those of the aforesaid expression 35.

More generally speaking, the reset processing is the processing in which Zi_rel_estm and Zj_rel_estm are determined to take values that are closer to zero than Zi_rel_estm_p and Zj_rel_estm_p are, while satisfying a condition in which the weighted average value of Zi_rel_estm and Zj_rel_estm is zero, that is, Wi*Zi_rel_estm+Wj*Zj_rel_estm=0. Incidentally, the reset processing includes a primary delay element, so that infinite time is required for the node relative floor height errors to completely return to zero. Hence, a finite stabilization function generator previously proposed by the present applicant (Japanese Unexamined Patent Application Publication No. H5-324115) may be used to gradually reset the node relative floor height errors to zero.

Further, if the modes of the two child nodes are “others,” that is, if the modes are neither “all ready” nor “all reset” in S6400 of FIG. 45 (for example, if the modes of the two child nodes are all hold modes), then the same processing as that of S6406 (refer to FIG. 48) is carried out in S6410.

As described above, only in a case where the modes of the two child nodes of the n-th node are all ready, and there is no danger in that the divergence takes place in the processing of estimating node relative floor height errors, the substantial estimation processing of the node relative floor height errors of the two child nodes is carried out. And, even if the modes of the child nodes are all ready, if there is a danger of the occurrence of divergence of estimation processing, then the values of the estimated node relative floor height errors of the two child nodes are held. Further, in a situation wherein the actual node floor reaction forces of the two child nodes are both zero, that is, if no actual floor reaction forces act on every ground contact portions 10 belonging to an n-th node, then the node relative floor height errors of the two child nodes are reset so that they are gradually reset to zero.

Referring now to FIG. 49 to FIG. 55, the three-child-node floor configuration estimation processing of S66 of FIG. 41 will be explained. FIG. 49 is a flowchart showing the subroutine processing of the three-child-node floor configuration estimation processing, FIG. 50 and FIG. 51 are flowcharts showing the subroutine processing of S6604 and S6608 of FIG. 49, FIG. 52 is a flowchart showing the subroutine processing of S6606, S6614, and S6618 of FIG. 49, and FIG. 53 and FIG. 54 are flowcharts showing the subroutine processing of S6612 and S6616, respectively, of FIG. 49.

In the three-child-node floor configuration estimation processing, as shown in the flowchart of FIG. 49, the modes of all child nodes of an n-th node having three child nodes (the modes determined in S58 of FIG. 41) are determined in S6600. The determination results are divided into six, namely, “all ready,” “all reset,” “only two child nodes are ready,” “only one child node is held, and the rest are reset,” “only two child nodes are held and the rest is reset,” and “others (else).”

Here, in the flowchart of FIG. 49, to explain more generally the floor configuration estimation processing for three child nodes, considerations are given not only to a case where all the ground contact portions 10 belonging to each intermediate node (the 145th node and the 236th node) simultaneously come in contact with the ground or leave a floor, as explained in the aforesaid second embodiment, but also to a case where a desired gait of a robot is generated such that one of the ground contact portions of each intermediate node is in contact with the ground, while the remaining ground contact portions leave the floor. In this case, unlike the two-child-node type, it is necessary to consider also when the modes of the three child nodes are “only two child nodes are ready,” “only one child node is held, and the remaining ones are reset,” or “only two child nodes are held, and the remaining are reset.” In the following explanation, the three child nodes of an n-th node will be referred to as an i-th node, a j-th node, and a k-th node.

Here, if it is determined that the modes of child nodes are “all ready,” then it is determined in S6602 whether a resultant force Fn_n (=Fi_act z+Fj_act_z+Fk_act_z) resultant force of the translational force vertical components Fi_act_z, Fj_act_z, and Fk_act_z of the actual node floor reaction forces of the child nodes of the n-th node is larger than a predetermined value Fn_min2. In other words, Fn_n denotes the translational force vertical component of the resultant force of the actual floor reaction forces of all ground contact portions belonging to the n-th node.

If the determination result of S6602 is YES, then intra-group complete estimation processing for a node having three child nodes (the processing for virtually estimating the node relative floor height error of each of the three child nodes owned by the n-th node) is carried out in S6604. In this processing, estimated node relative floor height errors Zi_rel_estm, Zj_rel_estm, and Zk_rel_estm of the i-th node, j-th node, and the k-th node, respectively, which are the three child nodes of the n-th node, are determined (updated) according to the expressions shown in the flowchart of FIG. 50. More specifically, Zi_rel_estm will be representatively explained. An i-th node relative floor height error correction amount candidate value Zi_inc_cand determined in the aforesaid S54 is added to a value Zi_rel_estm_p of Zi_rel_estm in the last control cycle thereby to determine a new estimated i-th node relative floor height error Zi_rel_estm. The same applies to the j-th node and the k-th node.

If the determination result of S6602 is NO, then it means that the accuracy of estimating a floor configuration error would be excessively deteriorated, so that no substantial estimation processing is carried out, and the intra-group total hold processing for a node having three child nodes (the processing for holding the estimated node relative floor height errors of the three child nodes of the n-th node without updating them) is carried out in S6606. In the hold processing, as shown by the expressions in the flowchart of FIG. 52, the values of the estimated node relative floor height errors Zi_rel_estm, Zj_rel_estm, and Zk_rel_estm of the i-th node, the j-th node, and the k-th node, respectively, are maintained at the values Zi_rel_estm_p, Zj_rel_estm_p, and Zk_rel_estm at the last control cycle.

Further, if it is determined in S6600 of FIG. 49 that the modes of the three child nodes are “all reset,” then intra-group total reset processing for a node having three child nodes (the processing for resetting the estimated node relative floor height error of each of the three child nodes owned by the n-th node) is carried out in S6608. In the reset processing, the estimated node relative floor height errors Zi_rel_estm, Zj_rel_estm, and Zk_rel_estm of the i-th node, the j-th node, and the k-th node, respectively, are updated according to the expressions in the flowchart of FIG. 51 such that they gradually approximate to zero. Incidentally, the meanings of ΔT and Testm in the expressions are the same as those of the aforesaid expression 35.

More generally speaking, the reset processing is the processing in which Zi_rel_estm, Zj_rel_estm, and Zk_rel_estm are determined to take values that are closer to zero than Zi_rel_estm_p, Zj_rel_estm_p, and Zk_rel_estm_p are, while satisfying a condition in which their weighted average value is zero, that is, Wi*Zi_rel_estm+Wj*Zj_rel_estm+Wk*Zk_rel_estm=0. Incidentally, the aforesaid finite stabilization function generator (Japanese Unexamined Patent Application Publication No. H5-324115) may be used to reset Zi_rel_estm_p, Zj_rel_estm_p, and Zk_rel_estm_p to zero.

Further, if it is determined in S6600 of FIG. 49 that the modes of the three child nodes are “only two child nodes are ready,” then it is determined in S6610 whether the translational force vertical component Fn_n of the resultant force of the actual node floor reaction forces of the child nodes of an n-th node (the resultant force of the actual floor reaction forces of all ground contact portions 10 belonging to the n-th node) is larger than the predetermined value Fn_min2.

If the result of the determination is YES, then the intra-group partial estimation processing for a node having three child nodes (the processing for substantially estimating the node relative floor height error of each of the three child nodes owned by the n-th node) is carried out in S6612. This processing is carried out as shown by the flowchart of FIG. 53. In this case, it is assumed that the i-th node is not in the ready mode, while the j-th node and the k-th node are in the ready mode.

First, in S66120, the mode of the i-th node is determined if the result of the determination is the hold mode, then a new i-th node relative floor height error correction amount candidate value Zi_inc_cand′ is determined to be zero in S66122, or if it is the reset mode, then the new i-th node relative floor height error correction amount candidate value Zi_inc_cand′ is determined in S66124 according to the expression in the figure. Zi_inc_cand′ determined in S66124 denotes an i-th node relative floor height error correction amount candidate value for gradually approximating Zi_rel_estm_p to zero. The meanings of ΔT and Testm in the expressions of S66124 are the same as those of the aforesaid expression 35. Supplementally, the aforesaid finite stabilization function generator (Japanese Unexamined Patent Application Publication No. H5-324115) may be used to determine Zi_inc_cand′ in S66124.

Subsequently, in S66126, a new j-th node relative floor height error correction amount candidate value Zj_inc_cand′ and a new k-th node relative floor height error correction amount candidate value Zk_inc_cand′ are determined according to the expressions in the figure.

More specifically, Zj_inc_cand′ and Zk_inc_cand′ are determined such that they satisfy a condition represented by Wi*Zi_inc_cand′+Wj*Zj_inc_cand′+Wk*Zk_inc_cand′=0 (a condition in which the weighted average value of Zi_inc_cand′, Zj_inc_cand′ and Zk_inc_cand′ is zero) and a condition represented by Zj_inc_cand′−Zj_inc_cand=Zk_inc_cand′−Zk_inc_cand.

Subsequently, in S66128, Zi_inc_cand′, Zj_inc_cand, and Zk_inc_cand′ determined as described above are added to the values Zi_rel_estm_p, Zj_rel_estm_p, and Zk_rel_estm_p of the relative floor height errors of the i-th node, the j-th node, and the k-th node at the last control cycle so as to determine new Zi_rel_estm, Zj_rel_estm, and Zk_rel_estm.

By determining Zi_rel_estm, Zj_rel_estm, and Zk_rel_estm as described above, Zi_rel_estm, Zi_rel_estm, and Zk_rel_estm will be determined such that Zj_rel_estm−Zk_rel_estm approximates to Zj_inc_cand−Zk_inc_cand while satisfying Wi*Zi_rel_estm+Wj*Zi_rel_estm+Wk+Zk_rel_estm=0 at the same time.

If the result of the determination of S6610 of FIG. 49 is NO, then the accuracy of estimating a floor configuration error would be excessively deteriorated, so that no substantial estimation processing is carried out, but the same processing as that of the aforesaid S6606 (refer to FIG. 52) is carried out in S6614.

Further, if it is determined in S6600 of FIG. 49 that the modes of the three child nodes are “only one child node is held and the rest are reset,” then the processing for a case where only one child mode is hold and the rest are reset is carried out in S6616. In the processing, new node relative floor height errors Zi_rel_estm, Zj_rel_estm, and Zk-rel-estm are determined according to the expressions shown in the flowchart of FIG. 54. Incidentally, in this case, it is assumed that the mode of the i-th node is the hold mode and the modes of the j-th node and the k-th node are the reset mode. The meanings of ΔT and Testm in the expressions are the same as those in the aforesaid expression 35.

The processing of FIG. 54 is, more generally, the processing for determining Zi_rel_estm to be zero, and determining Zj_rel_estm and Zk_rel_estm to take values that are closer to zero than Zj_rel_estm_p and Zk_rel_estm_p are, while satisfying Wi*Zi_rel_estm+Wj*Zj_rel_estm+Wk*Zk_rel_estm=0 (a condition in which the weighted average value of Zi_rel_estm, Zj_rel_estm and Zk_rel_estm is zero) . Supplementally, by the moment the modes of the j-th node and the k-th node both become the reset mode, their node weights Wj and Wk should have become zero. Hence, based on a condition in that the sum of the weights Wi, Wj and Wk is 1, Wi will be 1 by that moment and Wi*Zi_rel_estm+Wj*Zj_rel_estm+Wk*Zk_rel_estm=0, so that Zi_rel_estm will be zero by that moment. Thus, holding the value of the i-th node Zi_rel_estm means maintaining the value at zero. Therefore, in the processing of FIG. 54, the value of Zi_rel_estm is determined to be zero.

Incidentally, the aforesaid finite stabilization function generator (Japanese Unexamined Patent Application Publication No. H5-324115) may be used in the processing for gradually resetting Zj_rel_estm and Zk_rel_estm to zero.

Further, if it is determined in S6600 of FIG. 49 that the modes of the child nodes are “only two child nodes are held and the rest is reset,” then the processing for a case where only two child modes are hold and the rest is reset is carried out in S6618. In the processing, new node relative floor height errors Zi_rel_estm, Zj_rel_estm, and Zk_rel_estm are determined according to the expressions shown in the flowchart of FIG. 55. Incidentally, in this case, it is assumed that the modes of the i-th node and the j-th mode are both the hold mode and the mode of the k-th node is the reset mode. The meanings of ΔT and Testm in the expressions are the same as those in the aforesaid expression 35.

The processing of FIG. 55 is, more generally, the processing for holding Zi_rel_estm and Zj_rel_estm at the values at the last control cycle, and determining Zk_rel_estm to take a value that is closer to zero than Zk_rel_estm_p is, while satisfying Wi*Zi_rel_estm+Wj*Zj_rel_estm+Wk*Zk_rel_estm=0 (a condition in which the weighted average value of Zi_rel_estm, Zj_rel_estm and Zk_rel_estm is zero) . Supplementally, by the moment the mode of the k-th mode becomes the reset mode, Wk should have become zero.

Incidentally, the aforesaid finite stabilization function generator (Japanese Unexamined Patent Application Publication No. H5-324115) may be used in the processing for gradually resetting Zk_rel_estm to zero.

Further, if it is determined to be “Others” in S6620 of FIG. 49 (e.g., if the modes of the three child nodes are all hold mode), then the same processing as that of the aforesaid S6606 (refer to FIG. 52) is carried out in S6620.

As described above, only in a case where the modes of two or more child nodes are ready out of the modes of the three child nodes of the n-th node, and there is no danger in that the divergence takes place in the processing of estimating node relative floor height errors, the substantial estimation processing of the node relative floor height errors of the two or more child nodes is carried out. And, even if the modes of two or more child nodes are all ready, if there is a danger of the occurrence of divergence of estimation processing, then the values of the estimated node relative floor height errors of the three child nodes are held.

As described above, in S60 to S66 of FIG. 41, estimated n-th relative floor height errors Zn_rel_estm(n=1,2, . . . , last node number) are determined. Incidentally, the relative floor height error of the root node is zero.

In the processing of the flowchart of FIG. 41, lastly, in S72, the sum of the estimated relative floor height errors of all ancestor nodes of an n-th node (n=1, 2, . . . , last leaf node number) and an estimated n-th node relative floor height error is determined, and the determined sum is defined as an estimated n-th ground contact portion floor height error (estimated n-th floor height error) Zfn_estm.

The above is the subroutine processing of the estimation processing of floor height errors in FIG. 40.

Next, returning to the explanation of FIG. 35, the estimated n-th floor height error Zfn_estm(n=1, 2, . . . , last leaf node number) sequentially determined by the floor configuration estimator 130 as described above is added to the corrected desired ground contact portion position/posture of each ground contact portion 10 by an adder 132 so as to determine corrected desired ground contact portion position/posture with floor configuration error compensation. Then, the corrected desired ground contact portion position/posture with floor configuration error compensation is input, in place of corrected desired ground contact portion position/posture, to the corrected desired ground contact portion position/posture with deformation compensation calculator 114 h.

The processing of the control device 50 including the floor configuration estimator 130 and the adder 132 explained above is repeatedly carried out at each control cycle of the control device 50, the estimated n-th floor height error Zfn_estm converges to an actual n-th floor height error. Further, desired ground contact portion positions/postures are corrected on the basis of the estimated n-th floor height errors Zfn_estm corresponding to the ground contact portions so as to absorb an influence exerted by an actual n-th floor height error on an actual floor reaction force.

If all n-th ground contact portions (n=1, 2, . . . , last leaf node number) are in contact with the ground, then the point corresponding to a desired n-th ground contact portion floor reaction force central point Qn on the actual ground contact surface of an n-th ground contact portion should agree with an actual n-th floor contact point Dn_act. Therefore, the aforesaid instantaneous n-th node relative floor height error Zn_rel′ should not change. In actuality, however, the instantaneous n-th node relative floor height error Zn_rel′ obtained by the aforesaid calculation algorithm of the aforesaid floor configuration estimator 130 significantly changes due to the vibrations and electrical noises of a mechanical system or control device of the robot 1. Therefore, if the instantaneous n-th node relative floor height error is directly used as an estimated n-th node relative floor height error, and based on this, the estimated n-th floor height error Zfn_estm is determined (that is, the sum of the instantaneous relative floor height errors of all ancestor nodes of an n-th node (n=1, 2, . . . , last leaf node number) and the instantaneous n-th node relative floor height error is defined as the estimated n-th floor height error Zfn_estm), and further, the result obtained by adding the estimated n-th floor height error Zfn_estm to the corrected desired ground contact portion position/posture is supplied to the corrected desired ground contact portion position/posture with deformation compensation calculator 114 h, as shown in FIG. 35, then the ground contact portions 10 of the robot 1 will oscillate or severely vibrate.

Meanwhile, in the intra-group complete estimation processing for a node having two child nodes, the intra-group complete estimation processing for a node having three child nodes, and the intra-group partial estimation processing for a node having three child nodes described above, the transfer function from the instantaneous n-th node relative floor height error Zn_rel′ to the estimated n-th node relative floor height error Zn_rel_estm will be a primary delay low-pass filter. In other words, the estimated n-th node relative floor height error Zn_rel_estm will be the one obtained by passing the instantaneous n-th node relative floor height error Zn_rel′ through a low-pass filter. Therefore, even when the estimated n-th floor height error Zfn_estm determined on the basis of the estimated n-th node relative floor height error Zn_rel_estm is added to the corrected desired ground contact portion position/posture, as shown in FIG. 35, the oscillation or vibration of the ground contact portions 10 will hardly occur. This makes it possible to restrain the ground contact portions 10 from oscillating or vibrating.

Incidentally, in the present embodiment, the time constant of the aforesaid low-pass filter is Testm. Other than by the processing of the floor configuration estimator 130 explained in the present embodiment, it is possible to accomplish a construction in which the transfer function from the instantaneous n-th node relative floor height error Zn_rel′ to the estimated n-th node relative floor height error Zn_rel_estm will be a low-pass filter.

The characteristics of the floor configuration estimator 130 in the present embodiment will now be explained.

Even if each node compensating angle is changed and the corrected desired ground contact portion position/posture with deformation compensation, which are the final desired ground contact portion position/posture, are changed to generate node compensating moments required for the stabilization control of the posture of the robot 1, the point corresponding to the desired n-th ground contact portion floor reaction force central point Qn of the n-th ground contact portion 10 on an actual ground contact surface remains in agreement with the actual n-th floor contact point Dnact as long as the ground contact portions 10 are actually in contact with the ground. Instead, the compliance mechanism 42 or the like of each leg deforms or the position/posture of the body 24 change. In other words, the compliance mechanism 42 or the like or the position/posture of the body 24 change in order to cancel the change of the corrected desired ground contact portion position/posture with deformation compensation.

The floor configuration estimator 130 in the present embodiment cancels the influences exerted on the estimated n-th floor height error Zfn_estm by the deformation amount of the compliance mechanism 42 or the like, which is calculated by the mechanism compliance model 134 on the basis of an actual floor reaction force, and the body posture inclination error θberr by the time the estimated n-th floor height error Zfn_estm(n=1, 2, . . . , last leaf node number) is calculated from the corrected desired ground contact portion position/posture with deformation compensation. Hence, even if a body posture inclination error occurs or corrected desired ground contact portion position/posture with deformation compensation change, the estimated n-th floor height error Zfn_estm is not influenced thereby.

This means that the processing for estimating a floor configuration is not subjected to interference by the compliance control or posture control of the robot 1. Therefore, even when the estimated n-th floor height error Zfn_estm is added to the corrected desired ground contact portion position/posture, as shown in FIG. 35, the stability allowance (resistance to oscillation) of the control device of the robot 1 hardly reduces. In other words, control and processing will not interfere with each and cause oscillation even if the compliance control, posture control and the floor configuration estimation processing and the operation for correcting a desired gait of the robot 1 by using the estimated value of a floor configuration error are simultaneously performed. This means, in brief, that the leg compensating operation to cancel the influence exerted on a floor reaction force by a floor configuration error (the operation for correcting the position/posture of a ground contact portion) can be simultaneously performed while simultaneously estimating a plurality of parameters of the floor configuration error. Moreover, the leg compensating operation for posture control (the operation for correcting the position/posture of a ground contact portion) can be simultaneously performed.

Further, if the estimated n-th floor height error Zfn_estm of each ground contact portion 10 is added to the corrected desired ground contact portion position/posture, as shown in FIG. 35, then even if there is actually an n-th floor height error, the influence thereof can be offset, so that the actual total floor reaction force will steadily be the same as that in a case where the robot 1 is traveling on a supposed floor. Furthermore, the estimation of the estimated n-th floor height error Zfn_estm and the correction of corrected desired ground contact portion position/posture by using the estimated n-th floor height error Zfn_estm are sequentially carried out for each control cycle; therefore, even if a floor configuration changes on the way, the n-th floor height error after the change can be estimated and the influence exerted by the change in the floor configuration can be cancelled on the basis of the estimated value. However, the floor configuration estimator 130 includes a low-pass filter, as mentioned above, so that an actual total floor reaction force is transitionally subject to the influence of a floor configuration error immediately after the ground contact portions 10 come in contact with the ground or if the floor configuration changes on the way. Thereafter, however, the influence is attenuated by the time constant Testm.

Further, there are the following characteristics.

-   1) Control is resistant to failure even if a floor configuration     error is large. This is because the hierarchical compliance control     does not carry out approximation limited to minute compensating     angles. -   2) The accuracy of estimating floor configuration errors is high. -   3) Resistance to the occurrence of oscillation attributable to the     interference with the robot posture stabilization control,     permitting a shorter time constant for estimation processing to be     achieved. This makes it possible to enhance the responsiveness of     the leg compensating operation to offset the influence exerted by a     floor configuration error on a floor reaction force, so that the     transitional influence exerted by the floor configuration error on     the floor reaction force can be quickly eliminated.

If there is no correlation in the relationship between the estimated n-th floor height error in a landing period of one step before (a period in which a certain ground contact portion 10 comes in contact with the ground) and the estimated n-th floor height error in a landing period following the above landing period of one step before when a robot is traveling, then it is preferred to forcibly and gradually converge the estimated n-th floor height error to zero, as shown in the present embodiment. However, if, for example, it is known that the undulation of an actual floor surface not considered in a supposed floor is gentle, then it is considered that there is some correlation in the aforesaid relationship. In this case, the estimated n-th floor height error in the current landing period may be forcibly and gradually converged to the value obtained by multiplying the estimated n-th floor height error in the landing period of one step before by a positive constant that is smaller than 1.

Further, the convergence desired value of an estimated n-th floor height error in the current landing period may be determined by using not only the estimated n-th floor height error in the landing period of one step before but also the estimated n-th floor height error in the landing period of a plurality of steps before. Further, the convergence desired value may be determined by using the estimated n-th node floor height error and the estimated n-th node floor inclination error of another node in the landing period of current step, one step before or a plurality of steps before.

Since the present embodiment (the third embodiment) has been constructed as described above, the estimation of floor configurations, specifically, the floor height errors associated with ground contact portions, can be simultaneously, in other words, compositely, estimated with high accuracy even in a robot having many ground contact portions, which has been difficult to accomplish with conventional technologies. Moreover, if the ground contact portions of a robot are provided such that their postures can be controlled, and floor inclination errors are estimated in addition to floor height errors in the processing for estimating floor configurations, the floor inclination errors can be also simultaneously estimated with high accuracy.

Furthermore, by correcting a desired ground contact portion position/posture trajectory on the basis of the result of estimating a floor configuration, even if a floor configuration is different from a supposed configuration, the influence thereof can be absorbed, making it possible to generate a floor reaction force as desired.

In particular, the steady error of an actual floor reaction force from a control desired value, which has not been completely removed by the hierarchical compliance control that does not include corrections based on the estimated values of floor configuration errors, can be approximated to zero as much as possible. In other words, the steady errors of floor reaction forces caused by floor configuration errors can be eliminated.

Furthermore, the arrangement has been made such that the estimation of a floor configuration is interrupted in a situation wherein the accuracy of estimating a floor configuration may deteriorate or a situation wherein the estimated value thereof may diverge. This adds to the aforesaid advantages by preventing estimated values from being inappropriate.

Incidentally, the floor configuration estimator 130 of a robot according to the present embodiment is expected to estimate floor configurations by the aforesaid techniques, but it is not essential for the floor configuration estimator 130 to correct motions on the basis of estimated values.

Fourth Embodiment

A control device (a floor configuration estimator in particular) of a mobile robot according to a fourth embodiment of the present invention will now be explained. In the fourth embodiment, to explain it in brief, the block diagram of the floor configuration estimator 130 shown in FIG. 40 of the aforesaid third embodiment has been equivalently converted. More specifically, the floor reaction force of each node is estimated on the basis of the estimated value of a floor configuration estimated immediately before (the past value of an estimated value, such as the estimated value in the last control cycle), a desired gait that has been finally corrected and determined (detailedly, corrected desired ground contact portion position/posture with deformation compensation including a compliance operation)(or an actual joint displacement), and an actual body posture inclination error θberr. Based on the difference between the estimated value and the actual floor reaction force of each node, the correction amount candidate value of the estimated value of the floor configuration is determined to approximate the difference to zero. If it is determined that the estimation of the floor configuration will not diverge, then the estimated value of the floor configuration that has been estimated immediately before is corrected on the basis of the correction amount candidate value of the estimated value of the floor configuration. This processing corresponds to a hierarchical floor configuration estimating means that is an advanced version of the meaning for estimating a floor inclination related to each foot such that the difference between the supposed moment about the floor reaction force central point of the foot of each leg of a biped mobile robot and an actual moment approximates to zero and the means for estimating the interference angle between both legs such that the difference between the supposed moment about a total floor reaction force central point and an actual moment approximates to zero in the Japanese Unexamined Patent Application Publication No. H10-277969 previously proposed by the present applicant.

Thus, the present embodiment differs from the third embodiment only in the processing of a floor configuration estimator 130, so that the explanation of the processing of those other than the floor configuration estimator 130 will be omitted.

FIG. 57 is a block diagram showing the functional means of the floor configuration estimator 130 in the present embodiment. Referring to this FIG. 57, the floor configuration estimator 130 in the present embodiment will be explained below.

First, based on corrected desired n-th ground contact portion position/posture with deformation compensation (n=1, 2, . . . , last leaf node number) of each ground contact portion 10, the desired n-th ground contact portion floor reaction force central point of the ground contact portion 10 observed from the desired ground contact portion position of each ground contact portion 10, and the aforesaid posture inclination error θberr, the corrected desired n-th ground contact portion position/posture with deformation compensation is rotationally moved by θberr, about a desired total floor reaction force central point being the center of the rotation, so as to determine the corrected desired n-th ground contact portion position/posture (n=1, 2, . . . , last leaf node number) with deformation compensation after rotation. A supposed n-th floor contact position Dn(n=1, 2, . . . , last leaf node number) on a desired gait is subtracted from the determined corrected desired n-th ground contact portion position/posture (n=1, 2, . . . , last leaf node number) with deformation compensation after rotation so as to determine an n-th ground contact portion interference height Zn_int. This is supplied to a mechanism compliance model (inverse model) provided in a floor configuration estimator 130, thereby determining an estimated n-th ground contact portion floor reaction force, which is the estimated value of the floor reaction force of each ground contact portion 10.

Incidentally, the mechanism compliance model here is used to determine the mutual relative relationship among the estimated ground contact portion floor reaction forces on the basis of the mutual relative relationship among ground contact portion interference heights, and it finds no meaning in absolute values. Further, if the detected value of an actual floor reaction force Fn_act of an n-th ground contact portion is smaller than a certain threshold value Fn_min3, then it is assumed that the n-th ground contact portion is not in contact with the ground also in the aforesaid mechanism compliance model.

Further, the aforesaid estimated n-th ground contact portion floor reaction force is subtracted from the detected value of the actual floor reaction force Fnact of an n-th ground contact portion (n=1, 2, . . . , last leaf node number) to determine the estimated error of the n-th ground contact portion floor reaction force Ffn_estm_err. The estimated error of the n-th ground contact portion floor reaction force Ffn_estm_err is expressed in terms of a force, so that it is converted into a height error by a conversion value Cn (e.g., the reciprocal of a spring constant) and the result is adopted as an n-th ground contact portion floor height error correction amount candidate value Zfn_inc_cand. Incidentally, the conversion value Cn is not necessarily a diagonal matrix.

Subsequently, hierarchical relativization is performed by the hierarchical relativization processing described above on the basis of the determined n-th ground contact portion floor height error correction amount candidate value Zfn_inc_cand so as to determine an n-th node relative floor height error correction amount candidate value Zn_inc_cand(n=1, 2, . . . , last node number). Then, based on the n-th node relative floor height error correction amount candidate value Zn_inc_cand, an estimated floor height error (estimated n-th floor height error) Zfn_estm(n=1, 2, . . . , last leaf node number) is determined by the subroutine processing of the estimation processing of floor height errors. The subroutine processing of the estimation processing of floor height errors here is identical to the processing of S56 to S72 shown in FIG. 41.

It is desirable to carry out the ground contact portion floor inclination error estimation processing as explained in conjunction with S70 of FIG. 41 in the case of a robot that has feet whose postures can be controlled as ground contact portions and is capable of estimating the floor inclination errors in ground contact portions, as disclosed in Japanese Unexamined Patent Application Publication No. H10-277969.

Here, the third embodiment and the fourth embodiment (the present embodiment) will be compared. The fourth embodiment is, in brief, the converted equivalent of the block diagram of the floor configuration estimator 130 of the third embodiment. Hence, the advantages of the fourth embodiment are the same as the advantages of the third embodiment. Further, as with the third embodiment, it is not essential to correct motions on the basis of the estimated values of floor configurations.

In the third and the fourth embodiments, distributed pressure sensors may be used as floor reaction force sensors. Distributed pressure sensors make it possible to estimate more finely the deformations of ground contact portions due to actual floor reaction forces than force sensors, such as the six-axis force sensor 34, so that the accuracy in estimating floor configurations can be improved.

Furthermore, as shown in FIG. 1 of Japanese Patent No. 3035051 by the present applicant, a plurality of contact sensors may be disposed on a ground contact portion (e.g., on the four corners of the ground contact portion), and it may be determined in which direction the ground contact portion is likely to float on the basis of the output signals thereof. Then, the estimation of a floor configuration in the direction in which a contact sensor that is not in contact with the ground is further likely to float may be interrupted, or the estimation of a floor configuration may be also interrupted when all contact sensors are floating.

Fifth Embodiment

In the first to the fourth embodiments described above, the explanations have been given to the cases where the total number of child nodes of a node having child nodes is three or less have been taken as examples; however, also in a case where the number of child nodes is four or more, the concepts of the moments and inclination angles can be expanded as shown below by introducing the concept of a weighted average.

The concept can be expanded for any of desired values, actual values, and errors. Here, therefore, the desired values, the actual values, and errors will not be discriminated; instead, the moment for a general set of ground contact portion floor reaction forces Fn(n=1,2, . . . ) will be expanded and defined as shown below.

A set of the floor reaction forces of an n-th ground contact portion Fn(n=1,2, . . . , last leaf node number) is hierarchically relativized to determine an n-th node relative floor reaction force Fn_rel(n=1,2, . . . , last node number).

The number of the child nodes of an n-th node is denoted by r.

The identification number of an j-th child node of the n-th node is denoted by aj (j=1,2, . . . ,r).

The weight of the j-th child node of the n-th node is denoted by Wj (j=1,2, . . . ,r).

The column vector having, as a j-th element, the weight of the j-th child node of the n-th node is denoted by UWn.

Thus, UWn is defined as shown by the following expression 38, where “T” means transposition, and a column vector can be expressed by transpose a row vector. UWn=(Wa1,Wa2, . . . ,War)^(T)   Expression 38

r−1 mutually independent vectors orthogonal to the vector UWn (that is, the scalar product with the vector UWn is zero) are denoted by R(1),R(2), . . . ,R(r−1). R(j) (j=1,2, . . . ,r−1) denotes the column vector of r row, 1 column. Incidentally, R(j) (j=1,2, . . . ,r−1) are desirably orthogonal to each other, considering the ease of computation and control accuracy. A matrix having R(j) set to a j-th column (j=1,2, . . . ,r−1) is denoted by Hn. Hn is the matrix of r row, r−1 column.

A column vector whose j-th element (j=1,2, . . . ,r) is a relative floor reaction force Faj_rel of a j-th child node of the n-th node is defined as an n-th group relative floor reaction force vector Fn_rel_c.

In other words, Fn_rel_c is defined by the following expression 39. Fn _(—) rel _(—) c=(Fa1_(—) rel, Fa2_(—) rel, . . . ,Far _(—) rel)^(T)   Expression 39 Fn_rel_c denotes a column vector of r row, 1 column.

A column vector Mn_exp satisfying the following expression 40 is referred to as an n-th node expansion floor reaction force moment. Fn _(—) rel _(—) c=Hn*Mn _(—) exp   Expression 40 Mn_exp denotes a column vector of r−1 row, 1 column.

For the set of ground contact portion floor reaction forces Fn(n=1,2, . . . ), the n-th node expansion floor reaction force moment Mn_exp is defined as described above.

More specifically, based on the ground contact portion floor reaction force Fn(n=1,2, . . . ), a hierarchized node relative floor reaction force Fn_rel(n=1,2, . . . ) is determined, and further, a vector having, as its element, the coefficient of the linear combination when a vector (Fa1_rel,Fa2_rel, . . . ,Far_rel)^(T) having, as its elements, the relative floor reaction forces of all child nodes aj (j=a1,a2, . . . ,ar) of an n-th node is expressed by the liner combination of certain predetermined mutually independent vectors R(j)(j=1,2, . . . ,r−1) that are orthogonal to a vector (Wa1,Wa2, . . . ,War)^(T) having, as its elements, the weights of all child nodes of the n-th node is referred to as the n-th node expansion floor reaction force moment Mn_exp.

Incidentally, a j-th element (the element of a j-th row) of the n-th node expansion floor reaction force moment Mn_exp is referred to as the j-th component of the n-th node expansion floor reaction force moment.

Further, the n-th node expansion floor reaction force moment relative to the set of actual ground contact portion floor reaction forces Fn(n=1,2, . . . ) is referred to as the actual n-th node expansion floor reaction force moment Mn_exp_act.

Further, the n-th node expansion floor reaction force moment relative to the set of desired ground contact portion floor reaction forces Fn(n=1,2, . . . ) is referred to as the desired n-th node expansion floor reaction force moment Mn_exp_ref. The desired n-th node expansion floor reaction force moment is usually set to zero vector.

Further, the n-th node expansion floor reaction force moment relative to the set of ground contact portion floor reaction force errors Fn(n=1,2, . . . ) is referred to as the n-th node expansion floor reaction force moment error Mn_exp_err.

Similarly, the inclination angle relative to a set of general ground contact portion heights Zn(n=1,2, . . . ) is expanded and defined as follows.

A set of ground contact portion heights Zn(n=1,2, . . . ,last leaf node number) is hierarchically relativized to determine an n-th node relative height Zn_rel(n=1,2, . . . ,last node number).

A column vector whose j-th element (j=1,2, . . . ,r) is a relative height Zaj_rel of a j-th child node of the n-th node is defined as an n-th node relative height vector Zn_rel_c.

In other words, Zn_rel_c is defined by the following expression 41. Zn _(—) rel _(—) c=(Za1_(—) rel, Za2_(—) rel, . . . ,Zar _(—) rel)^(T)   Expression 41

A column vector θn_exp satisfying the following expression 42 is referred to as an n-th node expansion floor reaction force inclination angle. Zn _(—) rel _(—) c=Hn*θn _(—) exp   Expression 42

θn_exp denotes a column vector of r−1 row, 1 column.

More specifically, based on the ground contact portion height Zn(n=1,2, . . . ), a hierarchized node relative height Zn_rel(n=1,2, . . . ) is determined, and further, a vector having, as its element, the coefficient of the linear combination when a vector (Za1_rel,Za2_rel, . . . ,Zar_rel)^(T) having, as its elements, the relative heights of all child nodes aj (j=1,2, . . . ,r) of an n-th node is expressed by the liner combination of certain predetermined mutually independent vectors R(j)(j=1,2, . . . ,r−1) that are orthogonal to a vector (Wa1,Wa2, . . . ,War)^(T) having, as its elements, the weights of all child nodes of the n-th node is referred to as the n-th node expansion inclination angle θn.

By using the expansion floor reaction force moment and the expansion inclination angle defined as described above, the compliance control and the processing for estimating floor configurations are expanded almost as they are for a case where the number of the child nodes of an a-th node is four or more. The following will explain an example thereof as a fifth embodiment. In the fifth embodiment, the functional construction of a control device of a robot is the same as that shown in the aforesaid FIG. 2. In this case, the functional construction of a hierarchical compliance operation determiner may be the same as that explained with reference to the aforesaid FIG. 35 except for a compensating total floor reaction force moment distributor, a compensating angle determiner, and a floor configuration estimator.

[Expanding the Processing of the Compensating Total Floor Reaction Force Moment Distributor]

For example, the processing of the compensating total floor reaction force moment distributor in the hierarchical compliance operation determiner is expanded as shown below.

The expansion floor reaction force moment to be generated in addition to a desired n-th node expansion floor reaction force moment Mn_exp_rel (normally zero) to restore the posture of a robot is referred to as an n-th node compensating expansion floor reaction force moment Mn_exp_dmd.

Expression 43 given below holds between the n-th node compensating expansion floor reaction force moment Mn_exp_dmd and the n-th node compensating floor reaction force moment Mn_dmd. However, Cn_mech denotes a predetermined coefficient matrix (the matrix of 2 rows, r−1 column) determined by a desired node floor reaction force central point and the compliance characteristic of the mechanism of the robot. Mn _(—) dmd=Cn_mech*Mn _(—) exp _(—) dmd   Expression 43

Meanwhile, generalizing the aforesaid expression 10 provides the following expression 44. Mdmd=ΣMn _(—) dmd   Expression 44

where Σ means the total sum on all n-th nodes.

Hence, the compensating total floor reaction force moment distributor may determine the n-th node compensating expansion floor reaction force moment Mn_exp_dmd (n=1,2, . . . ) and the n-th node compensating floor reaction force moment Mn_dmd such that expression 43 and expression 44 are satisfied.

[Expanding the Processing of the Compensating Angle Determiner (θn Determiner)]

The processing of the n-th node compensating angle determiner in the fifth embodiment is carried out as follows. For example, in the processing of the θ145 determiner in the third embodiment (refer to FIG. 25 and FIG. 33), the processing until M145 act is determined is replaced by the processing for determining the aforesaid actual n-th node expansion floor reaction force moment Mn_exp_act on the basis of the actual floor reaction force of each ground contact portion, 145 is replaced by n, and the dimension of the compensation filter, the low-pass filter, and gain Kn in FIG. 33 may be expanded to the number obtained by subtracting 1 from the number of child nodes of an n-th node. The compensating angle θn of the n-th node can be determined by the processing that has been replaced as described above.

[Estimating a Floor Configuration by Using an Expansion Floor Reaction Force Moment]

As described below, the processing of the floor configuration estimator may partly incorporate a technique using the concept of the expansion floor reaction force moment.

In place of the processing shown in FIG. 56 of the floor configuration estimator explained in the aforesaid fourth embodiment, the processing shown in the block diagram of FIG. 57 is used to estimate floor configuration errors.

Referring to FIG. 57, this processing will be explained. First, an n-th node expansion floor reaction force moment estimation error Mn_exp_estm_err corresponding to an n-th ground contact portion floor reaction force estimation error Ffn_estm_err (n=1,2, . . . ) is determined according to the definition of the expansion moment from the difference between the estimated n-th ground contact portion floor reaction force calculated as explained in conjunction with the aforesaid FIG. 56 and the actual floor reaction force of the n-th ground contact portion (the set of differences for each node). Incidentally, the aforesaid vector R(j) required to determine the n-th node expansion floor reaction force moment estimation error is determined on the basis of the weight of each node.

Subsequently, as shown in expression 45 given below, the n-th node expansion floor reaction force moment estimation error Mn_exp_estm_err is multiplied by a predetermined coefficient matrix Kn_cmpl thereby to determine an n-th node expansion inclination angle correction amount candidate value θn_inc_cand, which is the candidate value of the correction amount of the n-th node expansion inclination angle. θn _(—) inc _(—) cand=Kn _(—) cmpl*Mn _(—) exp _(—) estm _(—) err   Expression 45

Incidentally, Kn_cmpl is not necessarily a diagonal matrix.

Subsequently, a coefficient for a vector R(j) (j=1,2, . . . r−1) is defined as the j-th element of θn_inc_cand to determine a vector obtained by linearly combining the vectors R(j). The determined vector is defined as the n-th node relative floor height error correction amount candidate value Zn_inc_cand, which is the candidate value of the correction amount of the n-th node relative floor height error.

In other words, Zn_inc_cand is determined according to the following expression 46. Zn _(—) inc _(—) cand=Hn*θn _(—) inc _(—) cand   Expression 46

As described above, the processing for determining the n-th node floor reaction force estimation error Fn_estm_err to the n-th node relative floor height error correction amount candidate value Zn_inc_cand may be replaced from the one shown in FIG. 56 to the one shown in FIG. 57.

The arithmetic processing of FIG. 57 other than the above is the same as the arithmetic processing of FIG. 56.

The subroutine processing of the floor height error estimation processing in the fifth embodiment requires the processing for a case where the number of child nodes of an n-th node is four or more. This processing expands the aforesaid floor configuration estimation processing for three child nodes (refer to FIG. 41), and if the ground contact portion corresponding to a child node j is likely to float and diverge, then the estimated j-th node relative floor height error Zj_rel_estm is maintained at the last value Zj_rel_estm_p (the j-th node relative floor height error correction amount candidate value Zj_inc_cand is set to zero), as with the floor configuration estimation processing for three child nodes. Further, the estimated node relative floor height errors Zk_rel_estm of the remaining child nodes are determined such that they approximate to the value obtained by adding a certain predetermined bias value c to the sum of the last estimated k-th node relative floor height error Zk_rel_estm_p and Zn_inc_cand. However, the bias value c is determined such that the weighted average of the estimated node relative floor height errors of all child nodes of the n-th node is zero.

As described above, the floor configuration estimation processing can be expanded also when there is a node having four or more child nodes. However, the arithmetic processing will be easier if a hierarchical structure is determined not to include four or more child nodes rather than carrying out such processing.

Sixth Embodiment

Referring now to FIG. 58 to FIG. 65, a control device for a mobile robot according to a sixth embodiment of the present invention will be explained. FIG. 58 and FIG. 59 are side views showing a legged mobile robot 51 according to the sixth embodiment (a bipedal mobile robot in the present embodiment), the portion of a knee joint 56, which will be discussed later, being in contact with a floor (kneeling). Incidentally, FIG. 58 schematically shows the robot 51 as a linear link assembly.

An object of the invention related to the present embodiment is to stably control the posture of a robot by manipulating a reaction force (external force) received from a floor, including an object fixed on the floor, in a state wherein a portion, such as a knee, an elbow, or a torso other than the distal portions of the legs or arms of the robot is in contact with the floor or the object fixed (set) on the floor (an object regarded as a material extended from the floor).

More specifically, the object is to permit posture control in a state wherein the robot is on its knees or in a state wherein it is sitting on a chair.

A conventional publicly known human-like robot is not provided with sensors for detecting floor reaction forces on, for example, its knees, so that it has been impossible to control the floor reaction forces acting on the knees. For instance, in the conventional human-like robot, when the robot is on its knees as shown in FIG. 58 and FIG. 59, the floor reaction forces acting on the right and left knees are dependent on (governed by) the concavities and convexities of a floor, and the joint angles of legs have not been adjusted on the basis of the concavities and convexities of the floor. Hence, there have been some cases where the portions of the robot in contact with the floor shake due to concavities and convexities of the floor, leading to unstable ground contact states of the ground contact portions. And, in such a state, an attempt to make the robot move or work sometimes has caused instability of the posture of the robot.

Further, when, for example, the robot stands up from the state wherein it is kneeling, there has been a danger of the robot falling down if an actual floor inclination is different from the inclination supposed in a desired gait, because the position of the center-of-gravity of the robot is not properly controlled and the robot tries to stand up with its body posture inclined.

Further, in a case where the robot sitting on a chair operates or works while maintaining its posture stable, it is preferred that not only the floor reaction forces acting on the feet of the legs of the robot but also the reaction forces acting from the chair onto the buttocks of the robot can be properly controlled so as to restore the posture of the robot to a proper posture when it stands up from the chair or sits onto the chair.

Conceptually, if an object, such as a chair, which is fixedly set on a floor is considered as a part of a floor, then the aforesaid reaction forces may be said to be floor reaction forces in a broad sense.

With the view of the problems with the conventional device as described above, the sixth embodiment solves the aforesaid problems, and in a state wherein portions of a mobile robot, such as the knees, elbows, the torso, and buttocks, other than the distal portions of legs or arms are in contact with a floor or an object considered as a material extended from the floor and subjected to reaction forces, it properly controls not only the reaction forces acting on the distal portions of the legs or arms of a robot but also the reaction forces acting on the portions other than the distal portions of the legs or arms, while using the control of the hierarchical compliance described above, thereby making it possible to keep the posture of the mobile robot stable.

The following will explain the sixth embodiment in more detail by taking the robot (bipedal mobile robot) 51 shown in FIG. 58 and FIG. 59 as an example.

First, the mechanical construction of the robot 51 will be explained. In the robot 51, two legs (link mechanisms) 52 (one leg being not shown) are extended from the bottom end of a body (base) 53. Further, two arms (link mechanisms) 54 (one arm being not shown) are extended from upper part of the body 53. Each leg 52 is provided with a hip joint 55, a knee joint 56, and an ankle joint 57 at its proximal portion adjacent to the body 53 (the portion connected to the body 53), its middle portion, and its distal portion, respectively, which are operated by actuators, such as electric motors, and a foot 58 is connected to the distal portion of each leg 52 through the ankle joint 57. Similarly, each arm 54 is provided with a shoulder joint 59, an elbow joint 60, and a wrist joint 61 at its proximal portion adjacent to the body 53 (the portion connected to the body 53), its middle portion, and its distal portion, respectively, which are operated by actuators, such as electric motors, and a hand 62 is connected to the distal portion of each arm 54 through the wrist joint 61. In this example, the joints 55, 56, and 57 of each leg 2 are joints having, for example, 3 degrees of freedom, 1 degree of freedom, and 2 degrees of freedom, respectively, and the foot 58 has 6 degrees of freedom relative to the body 53. Further, the shoulder joint 59, the elbow joint 60, and the wrist joint 61 of each arm 54 are constructed such that their degrees of freedom allow the hand 62 to have a degree of freedom of 6 degrees or more with respect to the body 53.

Although not shown, the body 53 of the robot 51 is provided with a control device 50 similar to that of the aforesaid first embodiment. Further, a head 63 is provided at the upper end of the body 53.

In this robot 1, in the state wherein the robot 1 is on its knees (particular posture state), as shown in FIG. 58 and FIG. 59, the portions of the foot 58 of each leg 52 and each knee joint 56 (more specifically, the surface portion of a link (shank link) connecting the knee joint 56 and the ankle joint 57 at near the knee joint 56. Hereinafter referred to simply as the knee) and the hand 62 of each arm 54 are ground contact portions. And, in the present embodiment, as shown in FIG. 59, the knee, which is a ground contact portion, is provided with a floor reaction force sensor 90 (load sensor). The floor reaction force sensor 90 is constructed of a main body (sensor part) 92 and a soft member (elastic member) 94, such as a sponge. The main body 92 is fixed to the knee (leg link), and the outside of the main body 92 is covered with a soft member (elastic member) 94. To enhance the accuracy of the compliance control in the knee, it is desirable to shape the surface (ground contact surface) of the soft member 94 into a round convex surface in addition to covering the knee with the soft member 94. This arrangement reduces the nonlinearity of the relationship between a corrective operation of a desired motion of the robot 51 and a floor reaction force, resulting in better control performance of the compliance control. Incidentally, although not shown, the foot 58 and the ankle joint 57 are connected through a floor reaction force sensor, such as a six-axis force sensor, and the compliance mechanism. Similarly, the hand 62 and the wrist joint 61 are connected through a floor reaction force sensor, such as a six-axis force sensor, and the compliance mechanism, which are not shown. The connecting constructions may be ones that are publicly known.

The floor reaction force sensor 90 of the ground contact portion of the knee joint may be any one of the following.

-   1) Sensor that detects only the translational force in the direction     perpendicular to the surface of a ground contact portion that comes     in contact with a floor (ground contact surface). -   2) Sensor that detects not only the translational force in the     direction perpendicular to the surface of a ground contact portion     that comes in contact with a floor (ground contact surface) but also     the translational force in a direction other than the direction     perpendicular to the contact surface. -   3) Distributed pressure sensor -   4) Sensor that detects moments in addition to translational forces     (e.g., six-axis force sensor)

Alternatively, a displacement sensor for detecting the deformation (distortion) of the aforesaid soft member 94 may be used in place of one that directly detects a load.

In the present embodiment, as the floor reaction force sensor 90, the sensor of the above 2) will be used. To further enhance the accuracy of the compliance control, the sensor of the above 3) or 4) may be used as the floor reaction force sensor 90. Incidentally, as the structure of the knee, a floor reaction force sensor may be added to a protective pad provided with a spring (a pad for protecting the knee), as disclosed in Japanese Unexamined Patent Application Publication No. 2000-62760 previously proposed by the present applicant.

In the present embodiment, the hierarchical structure as shown in FIG. 61 is set for the robot 51 on its knees as described above. Specifically, the right foot 58, the left foot 58, the right knee, the left knee, the right hand 62, and the left hand 62 as the ground contact portions are associated with a first node, a second node, a third node, a fourth node, a fifth node, and a sixth node, respectively, which are leaf nodes. Further, a 12th node having the first node and the second node as child nodes, a 34th node having the third node and the fourth node as child nodes, a 56th node having the fifth node and the sixth node as child nodes, and a 1234th node having the 12th node and the 34th node as child nodes have been set as intermediate nodes. Further, a 123456th node having the 1234th node and the 56th node as child nodes has been set as the root node. Further, according to the policy similar to that in the aforesaid first embodiment and the like, a desired floor reaction force central point Qn(n=1,2,3,4,5,6,12,34,56,1234,123456) is associated with each node, as shown in the figure. Incidentally, the desired floor reaction force central point Q123456 of the 123456th node, which is the root node, is identical to a desired total floor reaction force central point P.

In the present embodiment, the control device 50 is supposed to have a functional construction similar to that shown in the aforesaid FIG. 2.

In this case, the desired ground contact portion trajectory of a desired motion in a desired gait output by a gait generating device 100 in the present embodiment is constructed of the desired position/posture trajectory of each hand 62, the desired position/posture trajectory of each foot 58, and the desired position trajectory of each knee. In this case, the gait generating device 100 preferentially determines desired foot positions/postures (desired first and second ground contact portion positions/postures), desired hand positions/postures (desired fifth and sixth ground contact portion positions/postures ), and desired knee positions (desired third and fourth ground contact portion positions) so that the feet 58, the hands 62, and the knees come in contact with the ground on a supposed floor surface as required for the gait, then determines a desired ZMP (desired total floor reaction force central point) in a supporting polygon, which is a minimum convex polygon that includes the desired ground contact point (or a desired ground contact line or a desired ground contact surface) of each ground contact portion, and then desired body position/posture are determined by using a dynamic model of the robot 1 such that the desired foot positions/postures, the desired hand positions/postures, the desired knee positions, and the desired ZMP are satisfied.

If the head 63 can be moved with respect to the body 53, then the desired position/posture trajectory of the head 63 is included in a desired motion.

In the present embodiment, the arithmetic processing of the robot geometric model (inverse kinematics calculator) 110 shown in FIG. 2 differs from that of the first to the third embodiments, as will be discussed later.

Further, in the present embodiment, a part of the processing technique of a hierarchical compliance operation determiner 114 differs from the one explained in the first to the third embodiments described above. FIG. 62 shows the functional construction of the hierarchical compliance operation determiner 114 of the present embodiment. In the processing of this functional construction, major aspects that are different from the first to the third embodiments will be explained. A compensating total floor reaction force moment distributor 114 a determines and outputs not only the node compensating floor reaction force moments of the intermediate nodes and the root node, but also a node compensating floor reaction force moment Mn_dmd of an n-th node (n=1,2,5,6) of the leaf nodes, that is, the required floor reaction force moment (the required moment for restoring the posture) about a desired n-th ground contact portion floor reaction force central point (n=1,2,5,6). This is because, in the robot 51, moments can be generated about the desired floor reaction force central points of the feet 58 and the hands 62 as the ground contact portions. The technique for determining the node compensating floor reaction force moment Mn_dmd, including the required moments, may be the same technique explained in the first and the second embodiments described above. Further, in the present embodiment, the compensating total floor reaction force moment distributor 114 a determines and outputs a corrected desired n-th ground contact portion floor reaction force moment Mn_refmdfd(n=1,2,5,6) about a desired n-th ground contact portion floor reaction force central point (n=1,2,5,6), which is corrected by generating a node compensating floor reaction force moment about the desired floor reaction force central point of each corresponding node, together with a translational force component Fn_refmdfd(n=1,2,3,4,5,6) of a corrected desired ground contact portion floor reaction force. These are used by a deformation compensation amount calculator 114 n to calculate a deformation compensation amount.

A compensating angle determiner (the part enclosed by the virtual line in FIG. 62) in the present embodiment determines and outputs not only the compensating angles of the intermediate nodes and the root node but also the node compensating angle θn of an n-th node (n=1,2,5,6) among the leaf nodes. A θn determiner that determines these n-th node compensating angles θn(n=1,2,5,6) determines an n-th node compensating angle θn(n=1,2,5,6) by the same processing as that of the foot compensating angle determiner disclosed in Japanese Unexamined Patent Application Publication No. H10-277969. Incidentally, the technique for determining the compensating angles θn(n=12,34,56,1234,123456) of the intermediate nodes and the root node other than the above nodes may be the same as the technique explained in the first and the second embodiments described above.

Further, a corrected desired ground contact portion position/posture calculator 114 g in the present embodiment combines node rotational manipulations on the basis of each n-th node compensating angle θn(n=12,34,56,1234,123456) by the technique explained in the first and the second embodiments describe above so as to correct a desired ground contact portion position, and it further corrects the desired posture of an n-th ground contact portion (n=1,2,5,6), thereby determining the corrected desired ground contact portion position/posture of each ground contact portion.

Further, according to the present embodiment, a floor configuration estimator 130 determines an estimated n-th floor height error Zfn_estm(n=1,2,3,4,5,6) by the same technique as the technique explained in the aforesaid third embodiment and also estimates a ground contact portion floor inclination error of an n-th ground contact portion θfn(n=1,2,5,6). The estimation of the ground contact portion floor inclination error θfn is performed by the same processing as the processing for estimating foot floor inclinations by the floor configuration estimator disclosed in Japanese Unexamined Patent Application Publication No. H10-277969.

The processing techniques of the functional construction of the hierarchical compliance operation determiner 114 of the present embodiment other than the ones described above are the same as that in the third embodiment.

Except for the difference aspects explained above, the arithmetic processing of the control device 50 in the present embodiment is the same as that in the first to the third embodiments.

Next, the processing of the aforesaid robot geometric model 110 in the present embodiment will be explained in detail.

In the robot 51 of the present embodiment, the number of the degrees of freedom of the joints from the body 53 to the knees is limited to three (only 3 degrees of freedom in the hip joints 55); therefore, it is impossible to change only the difference in height between the right and left knees while strictly maintaining body position/posture, foot position/posture, the horizontal positions of the right and left knees, and the sum of the heights of the right and left knees at their desired values. In other words, if an attempt is made to forcibly change only the height difference between the right and left knees, then twist or slippage would inconveniently take place between ground contact portions (namely, the knees and the feet) and a floor.

Similarly, it is impossible to change only the difference in height between the right and left feet while strictly maintaining the body position/posture, the positions of both knees, the horizontal positions of both feet, and the sum of the heights of the right and left feet at their desired values. In other words, if an attempt is made to forcibly change only the height difference between the right and left feet, then twist or slippage would inconveniently take place between ground contact portions (namely, the knees and the feet) and a floor.

Similarly, it is impossible to change only the sum of the heights of the right and left knees while strictly maintaining the body position/posture, foot positions/postures, the horizontal positions of the right and left knees, and the height difference between the right and left knees at their desired values. In other words, if an attempt is made to forcibly change only the sum of the heights of the right and left knees, then the body position would be dislocated from a desired position even though twist or slippage may not take place between ground contact portions (namely, the knees and the feet) and a floor. As a result, the position of the total center-of-gravity of the robot 51 and an inertial force will deviate from desired values, leading to deteriorated stability of the robot 51.

This means that, in any case, the corrected desired ground contact portion position/posture with deformation compensation and desired body position/posture that have been determined by the hierarchical compliance operation determiner 114 cannot be strictly satisfied at the same time, so that twist or slippage takes place between the feet 58 or the knees of the robot 51 and the floor or the total center-of-gravity of the robot 51 and an inertial force deviate from their desired values, possibly resulting in deterioration of the stability of the robot 51 when the robot 51 is in the posture state shown in FIG. 58 and FIG. 59. Incidentally, the hands 62 among the ground contact portions have a degree of freedom of 6 degrees or more of freedom with respect to the body 53; therefore, no twist or slippage will occur between the hands 62 and the floor unless the robot 51 is made to take a posture in which it comes in contact with the floor through portions other than the hands 62 of the arms 54.

Hence, in the present embodiment, the posture or the position/posture of the body 53 are corrected on the basis mainly of a change in the height difference between the right and left knees while restraining, as much as possible, the occurrence of twist or slippage between the knees and the feet 58 among the ground contact portions of the robot 51 and the floor and also restraining, as much as possible, a change in the position of the center-of-gravity (especially the horizontal position) of the body 53.

FIGS. 63(a) and (b) show examples of the operations for correcting the posture of the body 53. FIG. 63(c) will be discussed later. These FIGS. 63(a), (b), and (c) omit showing the arms 54 and the head 63.

One of the operations for correcting the posture of the body 53 is the operation in which, from the state wherein the robot 51 is on its knees as shown in FIG. 63(a), the body 53 is rotated (rotated as indicated by an arrow y1) by using, as the axis of rotation, the trunk axis passing approximately the center-of-gravity G of the body 53, as shown in FIG. 63(b), on the basis of a change in the height difference between the right and left knees caused by compliance control.

Further, in the posture in which the robot 51 is on its knees, as shown in FIG. 63, a change in the height difference between the right and left feet 58 and 58 causes the height difference between the right and left knees to change by about half the change in the height difference between the right and left feet 58 and 58. Hence, the same operation as that of correcting the posture of the body 53 on the basis of a change in the height difference between the right and left knees is performed by only a half amount, as compared with the case of a change in the height difference between the right and left feet, on the basis of a change in the height difference between the right and left feet 58 and 58 caused by the compliance control.

Further, when correcting the posture of the body 53 as described above, the position and the posture of the body 53 are corrected on the basis of a change in the sum of the heights of the right and left knees caused by the compliance control while restraining the occurrence of twist or slippage between ground contact portions (the knees and the feet 58) and the floor as much as possible, and also restraining, as much as possible, a change in the position (especially the horizontal position) of the center-of-gravity G of the body 53 or in the inclination of a segment connecting the center-of-gravity G and a total floor reaction force central point.

FIG. 64 visually shows an operation for correcting the position and the posture of the body 53 on the basis of changes in the sum of the heights of the right and left knees. Specifically, from the posture of the robot 51 indicated by the dashed lines, as both knees are operated to move down by the compliance control to the posture of the robot 51 indicated by the solid lines, the bottom end portion (or the waist) of the body 53 is shifted forward, as indicated by an arrow y3, and the inclination of the body 53 is shifted backward (in the direction in which the body 53 rises), as indicated by an arrow y2. In other words, the body 53 is tilted backward while maintaining the position of the center-of-gravity G of the body 53 (or the position of a predetermined representative point of the body 53), especially the horizontal position thereof. Alternatively, the body 53 is tilted backward while maintaining the inclination of the segment connecting the center-of-gravity G and the desired total floor reaction force central point P. Further, as an operation for raising both knees is performed by the compliance control, the bottom end portion (or the waist) of the body 53 is shifted backward, inversely from the above, to shift the inclination of the body toward the front. In other words, the body 53 is tilted forward while maintaining the position of the center-of-gravity G of the body (or the position of the predetermined representative point of the body), especially its horizontal position. Alternatively, the body 53 is tilted backward while maintaining the inclination of the segment connecting the center-of-gravity G and the desired total floor reaction force central point P. Incidentally, Q1″ and Q3″ in FIG. 64 denote the desired floor reaction force central point of the foot 58 and the desired floor reaction force central point of the knee, respectively, after the position/posture of the body 53 has been corrected as described above. In this example, Q1″ is identical to a desired floor reaction force central point Q1 of the foot 58 before the correction.

The above is the overview of the operation for correcting the posture and the position of the body 53.

The processing function of the robot geometric model (inverse kinematics calculator) 110 in the present embodiment is shown by the block diagram of FIG. 65.

In the inverse kinematics calculator 110, first, based on corrected desired n-th ground contact portion position/posture with deformation compensation (n=1,2, . . . ,6) and the desired ground contact portion positions/postures of the feet 58 and the knees determined by the gait generating device 100, the correction amount of the height difference between the right and left knees Zkneediffmdfd and the correction amount of the sum of the heights of the right and left knees Zkneesummdfd for the desired positions of the right and left knees determined by the gait generating device 100, and the correction amount of the height difference between the right and left feet 58 and 58 Zfootdiffmdfd for the desired positions of the right and left feet determined by the gait generating device 100 are determined.

Specifically, the correction amount of the height difference between the right and left knees Zkneediffmdfd, the correction amount of the sum of the heights of the right and left knees Zkneesummdfd, and the correction amount of the height difference between the right and left feet Zfootdiffmdfd are determined according to the following expressions 47, 48, and 49. $\begin{matrix} {{Zkneeddiffmdfd} = {{{Corrected}\quad{desired}\quad{fourth}\quad{ground}\quad{contact}\quad{portion}\quad{position}\quad{with}\quad{deformation}\quad{compensation}} - {{Desired}\quad{fourth}\quad{ground}\quad{contact}\quad{portion}\quad{position}} - {{Corrected}\quad{desired}\quad{third}\quad{ground}\quad{contact}\quad{portion}\quad{position}\quad{with}\quad{deformation}\quad{compensation}} + {{Desired}\quad{third}\quad{ground}\quad{contact}\quad{portion}\quad{position}}}} & {{Expression}\quad 47} \\ {{{Zkneesummdfd} = {{{Corrected}\quad{desired}\quad{fourth}\quad{ground}\quad{contact}\quad{portion}\quad{position}{\quad\quad}{with}\quad{deformation}\quad{compensation}} - {{Desired}\quad{fourth}\quad{ground}\quad{contact}\quad{portion}}\quad + {{Corrected}\quad{desired}\quad{third}\quad{ground}\quad{contact}\quad{portion}\quad{position}{\quad\quad}{with}\quad{deformation}\quad{compensation}} - {{Desired}\quad{third}\quad{ground}\quad{contact}\quad{portion}\quad{position}}}}\quad} & {{Expression}\quad 48} \\ {{Zfootdiffmdfd} = {{{Corrected}\quad{desired}\quad{second}\quad{ground}\quad{contact}\quad{portion}\quad{position}\quad{with}\quad{deformation}\quad{compensation}} - {{Desired}\quad{second}\quad{ground}\quad{contact}\quad{portion}\quad{position}} - {{Corrected}\quad{desired}\quad{first}\quad{ground}\quad{contact}\quad{portion}\quad{position}\quad{with}\quad{deformation}\quad{compensation}} + {{Desired}\quad{first}\quad{ground}\quad{contact}\quad{portion}\quad{position}}}} & {{Expression}\quad 49} \end{matrix}$

More precisely, “portion position” in expressions 47 to 49 is the height component (the component in the vertical direction) of the portion position.

Subsequently, the body position/posture correction amount are determined on the basis of the correction amount of the height difference between the right and left knees Zkneediffmdfd, the correction amount of the sum of the heights of the right and left knees Zkneesummdfd, and the correction amount of the height difference between the right and left feet Zfootdiffmdfd.

The body position/posture correction amount is composed of a body position correction amount for knee height difference Xbkneediffmdfd, a body posture correction amount for knee height difference θbkneediffmdfd, a body position correction amount for knee height sum Xbkneesummdfd, a body posture correction amount for knee height sum θbkneesummdfd, a body position correction amount for foot height difference Xbfootdiffmdfd, and a body posture correction amount for foot height difference θbfootdiffmdfd. Specifically, these values are determined as follows.

First, based on the correction amount of the height difference between the right and left knees Zkneediffmdfd, the body position correction amount for knee height difference Xbkneediffmdfd and the body posture correction amount for knee height difference θbkneediffmdfd for preventing the correction amount of the height difference between the right and left knees Zkneediffmdfd from causing twist or slippage between ground contact portions (the knees and the feet 58) and a floor are determined by geometric calculation on the basis of the correction amount of the height difference between the right and left knees Zkneediffmdfd and the desired posture (desired motion) of the robot 51 at that instant (current time).

To be more specific, for example, Xbkneediffmdfd and θbkneediffmdfd are determined according to the following expression 50, where Kxkneediff and Kthkneediff denote proportional coefficients based on desired postures of the robot 51. Xbkneediffmdfd=Kxkneediff*Zkneediffmdfd θbkneediffmdfd=Kthkneediff*Zkneediffmdfd   Expression 50

As an alternative, the relationship between the correction amount of the height difference between the right and left knees (or a 34th node compensating angle θ34) and the correction amounts of the body position/posture may be determined beforehand for some representative desired postures of the robot 51, and it may be stored as a map or function so as to determine Xbkneediffmdfd and θbkneediffmdfd on the basis of the map or function and the correction amount of the height difference between the right and left knees Zkneediffmdfd. Incidentally, in the present embodiment, the body posture is corrected by rotating the body 53 about its trunk axis on the basis of the correction amount of the height difference between the right and left knees, so that the body position correction amount for knee height difference Xbkneediffmdfd may be zero.

Next, in the same manner as described above, based on the correction amount of the height difference between the right and left feet 58 and 58 Zfootdiffmdfd, the body position correction amount for foot height difference Xbfootdiffmdfd and the body posture correction amount for foot height difference θbfootdiffmdfd for preventing the correction amount of the height difference between the right and left feet 58 and 58 Zfootdiffmdfd from causing twist or slippage between ground contact portions (the knees and the feet 58) and a floor are determined by geometric calculation on the basis of the correction amount of the height difference between the right and left feet Zfootdiffmdfd and the desired posture (desired motion) of the robot 51 at that instant (current time).

To be more specific, for example, Xbfootdiffmdfd and θbfootdiffmdfd are determined according to the following expression 51, where Kxfootdiff and Kthfootdiff denote proportional coefficients based on desired postures of the robot 51. As described above, the influence of the height difference of the feet 58 is about half the influence of the height difference of the knees, so that Kxfootdiff and Kthfootdiff are one half of Kxkneediff and Kthkneediff. Xbfootdiffmdfd=Kxfootdiff*Zfootdiffmdfd θbfootdiffmdfd=Kthfootdiff*Zfootdiffmdfd   Expression 51

Incidentally, in the present embodiment, the body posture is corrected by rotating the body 53 about its trunk axis on the basis of the correction amount of the height difference between the right and left feet 58 and 58, so that the body position correction amount for foot height difference Xbfootdiffmdfd may be zero.

Subsequently, in the same manner as described above, based on the correction amount of the sum of the heights of the right and left knees Zsummdfd, the body position correction amount for knee height sum Xbsummdfd and the body posture correction amount for knee height sum θbsummdfd for preventing the correction amount of the sum of the heights of the right and left knees Zsummdfd from causing twist or slippage between ground contact portions (the knees and the feet) and a floor are determined by geometric calculation on the basis of the correction amount of the sum of the heights of the right and left knees Zsummdfd and the desired posture of the robot 51 at that instant (current time).

Alternatively, the relationship between the correction amount of the sum of the heights of the right and left knees and the correction amount of the body posture may be determined beforehand for some representative desired postures, and it may be stored as a map or function so as to determine Xbsummdfd and θbsummdfd on the basis of the map or function and the correction amount of the sum of the heights of the right and left knees Zsummdfd.

Alternatively, the correction amounts of body position/posture may be determined as follows. The height difference between the right and left knees is converted into the 34th node compensating angle θ34 that causes the difference, the sum of the heights of the right and left knees is converted into the 1234th node compensating angle θ1234, and the height difference between the right and left feet is converted into the 12th node compensating angle θ12 that causes the difference. Then, based on these converted compensating angles, the body position/posture correction amounts may be determined by geometric calculation. Alternatively, the relationship between the converted compensating angles and body position/posture correction amounts may be determined beforehand for some representative desired postures of the robot 51, and it may be stored as a map or function so as to determine body position/posture correction amounts on the basis of the map or function and the converted compensating angles.

Subsequently, the desired body position/posture are moved (rotational movement and parallel movement) by the body position/posture correction amounts to determine desired body position/posture with correction. Specifically, the desired body position/posture are rotationally moved about a trunk axis (or a predetermined axis of rotation (the axis of rotation substantially in a vertical plane)) by the sum of the body posture correction amount for knee height difference and the body posture correction amount for foot height difference (θbkneediffmdfd+θbfootdiffmdfd), and this is further rotationally moved about a lateral axis by the body posture correction amount for the sum of knee heights θbfootsummdfd, then this is further moved in parallel in the longitudinal direction of the robot 51 by the sum of the body position correction amount for knee height difference, the body position correction amount for the sum of knee heights, and the body position correction amount for foot height difference (Xbkneediffmdfd+Xbkneesummdfd+Xbfootdiffmdfd) thereby to determine desired body position/posture with twist correction.

Subsequently, based on the desired body position/posture with twist correction and desired n-th ground contact portion position/posture with deformation compensation (n=1,2, . . . ,6), a joint displacement command of the robot 51 is determined.

Thus, the processing of the inverse kinematics calculator 110 is carried out as described above.

In other words, according to the sixth embodiment, if the degree of freedom is geometrically insufficient for the operation of correcting the ground contact portion positions/postures of the robot 51, then the hierarchical compliance operation corrects not only the desired ground contact portion positions/postures but also the desired body positions/postures such that a point of the body position (the representative point of the body) or the point (e.g., the overall center-of-gravity) of the weighted average position of a plurality of portions, including the body (the weight in this case is desirably the mass ratio of each portion), especially the horizontal position of the point is maintained, or the angle of a line that connects the point and a desired total floor reaction force central point (desired ZMP) is maintained at the angle in a desired gait.

Further, in other words, the sixth embodiment is provide with a means that gives priority to the controllability of an actual floor reaction force moment generated about a desired total floor reaction force central point (desired ZMP) and determines at least one of a compensating height and a compensating angle as the manipulated variable of the relative height or the inclination angle of a predetermined ground contact portion A (a knee in this case) or the manipulated variable of the relative height or the inclination angle of a node having a predetermined ground contact portion A as a descendant node in order to enhance the stability of the entire posture control of the robot 51, a means for determining the correction amount of at least one of the posture and the position of the body while maintaining the position of the body or the aforesaid weighted average position of a plurality of portions, including the body, at approximately the position in a desired gait on the basis of at least one of the aforesaid compensating height and compensating angle, and an inverse kinematics calculating means for determining a joint displacement on the basis of at least one of the position and the posture of a predetermined ground contact portion B (foot in this case) except for the aforesaid predetermined ground contact portion A and the body position/posture corrected by the aforesaid correction amount.

Incidentally, in the sixth embodiment, instead of rotating a body posture about the trunk axis of the body 53 of the robot 51, the body 53 may be rotated using the vertical axis as the axis of rotation and using the waist (the bottom end portion of the body 53) of the robot 51 as the center, as shown in FIG. 64(c). In this case, however, the deviation of the center-of-gravity G of the body 53 in the lateral direction would increase. Alternatively, the body posture may be rotated using an axis located midway between the trunk axis of the body 53 and the vertical axis as the axis of rotation. Alternatively, the position and the posture of the body 53 may be simultaneously corrected on the basis of the correction amount of the height difference between both knees or the correction amount of the height difference between both feet 58 and 58. Further, instead of maintaining the position of the center-of-gravity G of the body 53 unchanged, the position/posture of the body 53 may be corrected such that the overall center-of-gravity position of the robot 51 or the position of the representative point of the body 53 remain unchanged.

In any case, if the degree of freedom is geometrically insufficient in the operation for correcting the position/posture of a ground contact portion of the robot 51 by the compliance operation or the like, at least one of the position and the posture of the body 53 may be corrected from the position/posture of a desired gait so as to restrain the occurrence of slippage, such as twist, of a ground contact portion. Further, instead of correcting at least one of the position and the posture of the body 53, at least one of the position and the posture of a predetermined portion other than the body 53 may be corrected.

Seventh Embodiment

An explanation will now be given about a control device for a mobile robot according to a seventh embodiment of the present invention.

In the sixth embodiment, the position/posture of the body has been corrected in order to prevent slippage, such as twist, of a ground contact portion and to minimize the deviations of the position of the overall center-of-gravity of the robot 51 and an inertial force. In the present embodiment (the seventh embodiment), slippage, such as twist, of a ground contact portion has been allowed to a certain extent, and a joint displacement (the displacement between a hip joint 55 and a knee joint 56) or the position/posture of a ground contact portion have been preferentially corrected so as to restrain the deviations of the position of the overall center-of-gravity of the robot 51 and an inertial force.

More specifically, in the present embodiment, an inverse kinematics calculator 110 carries out the processing shown by the block diagram of FIG. 66 in place of the processing of FIG. 65 explained in the sixth embodiment. Except for this difference, the present embodiment is the same as the sixth embodiment.

The inverse kinematics calculator 110 of the present embodiment will be explained with reference to FIG. 66. First, based on corrected desired n-th ground contact portion position/posture with deformation compensation (n=1,2, . . . ,6) (specifically, the position out of the position/posture) and desired n-th ground contact portion position/posture (n=1,2, . . . ,6) (specifically, the position out of the position/posture), a correction amount of the height difference between the right and left knees Zkneediffmdfd, a correction amount of the sum of the heights of the right and left knees Zkneesummdfd, and a correction amount of the height difference between the right and left feet Zfootdiffmdfd are determined according to the aforesaid expression 47, expression 48, and expression 49, and further, the height difference between the right and left knees is converted into a 34th node compensating angle θ34 that causes the difference. Similarly, the sum of the heights of the right and left knees is converted into a 1234th node compensating angle θ1234 that causes the sum, and the height difference between the right and left feet is converted into a 12th node compensating angle θ12 that causes the difference.

Next, based on the converted node compensating angles, joint displacement correction amounts are determined. This is performed as follows. First, a case is assumed where a joint displacement is corrected while fixing the body position/posture of a desired gait and retaining the angle of a segment that connects a desired total floor reaction force central point (a desired 123456th node floor reaction force central point (point P in FIG. 61)) and the overall center-of-gravity of the robot 51 (or the center-of-gravity of the body or the center-of-gravity of a plurality of portions, including the body 53) unchanged. In this case, a j-th joint displacement correction amount (j=1,2, . . . ,total number of joints) for the inclination angle (compensating angle) of an n-th node (n=12,34,1234) corresponding to each of ground contact portions (a knee and foot 58) to become a unit perturbation amount is determined, and this is defined as a sensitivity Ln_j of the j-th joint displacement relative to the n-th node compensating angle.

More generally, the j-th joint displacement correction amount (j=1,2, . . . ,total number of joints) for the relative angle between the segment connecting a desired total floor reaction force central point (a desired 123456th node floor reaction force central point) and an overall center-of-gravity of the robot 51 (or the center-of-gravity of the body or the center-of-gravity of a plurality of portions, including the body 53) and the inclination angle of an n-th node (n=12,34,1234) corresponding to each of ground contact portions (a knee and the foot 58) to become a unit perturbation amount is determined when a joint displacement of a desired gait has been corrected, and this is defined as a sensitivity Ln_j of the j-th joint displacement relative to the n-th node compensating angle.

Specifically, if Ln_knee_r denotes the sensitivity of a right knee joint displacement relative to an n-th node compensating angle (n=12,34,1234), Ln_knee_l denotes the sensitivity of a left knee joint displacement relative to the n-th node compensating angle, Ln_hip_r denotes the sensitivity of a right hip pitch joint displacement relative to the n-th node compensating angle, and Ln_hip_l denotes the sensitivity of a left hip pitch joint displacement relative to the n-th node compensating angle, then each sensitivity is set according to the following expression 52 in the posture of the robot 51 on its knees shown in FIG. 58 and FIG. 59. L1234_hip_r = 0, L1234_hip_1 = 0, L1234_knee_r = 1, L1234_knee_1 = 1, L12_hip_r = 0, L12_hip_1 = 0 L12_knee_r = a12, L12_knee_1 = −a12, L34_hip_r = a34 L34_hip_1 = −a34, L34_knee_r = 0, L34_knee_1 = 0                    . . . Expression 52 where a12 and a34 denote predetermined constants.

Subsequently, joint displacement correction amounts are determined according to expressions 53 to 56 given below, where θknee_r denotes a right knee joint displacement correction amount, θknee_l denotes a left knee joint displacement correction amount, θhip_r denotes a right hip joint displacement correction amount (more specifically, the joint displacement correction amount in the pitch direction of the right hip joint), and θhip_l denotes a left hip joint displacement correction amount (more specifically, the joint displacement correction amount in the pitch direction of the right hip joint). $\begin{matrix} {{\theta\quad{knee\_ r}} = {{{L1234\_ knee}{\_ r}*\theta\quad 1234} + {{L12\_ knee}{\_ r}*\theta\quad 12} + {{L34\_ knee}{\_ r}*\theta\quad 34}}} & {{Expression}\quad 53} \\ {{\theta knee\_ l} = {{{L1234\_ knee}\_ 1*{\theta 1234}} + {{L12\_ knee}{\_ l}*{\theta 12}} + {{L34\_\_ knee}{\_ l}*{\theta 34}}}} & {{Expression}\quad 54} \\ {{\theta\quad{hip\_ r}} = {{{L1234\_ hip}{\_ r}*\theta\quad 1234} + {{L12\_ hip}{\_ r}*\theta\quad 12} + {{L34\_ hip}{\_ r}*\theta\quad 34}}} & {{Expression}\quad 55} \\ {{\theta hip\_ l} = {{{L1234\_ hip}\_ 1*{\theta 1234}} + {{L12\_ hip}{\_ l}*{\theta 12}} + {{L34\_\_ hip}{\_ l}*{\theta 34}}}} & {{Expression}\quad 56} \end{matrix}$

Other joint displacement correction amounts are determined in the same manner; however, it will be omitted to simplify the explanation.

For the 1234th compensating angle θ1234, the displacements (angles) of both knee joints 56 are corrected by the same amount as θ1234, as shown in FIG. 67. Further, for the 12th compensating angle θ12, the displacement (angle) of the right knee joint 56 is corrected in proportion to θ12 and also the displacement (angle) of the left knee joint 56 is corrected at the same time by the amount obtained by multiplying the correction amount of the displacement of the right knee joint 56 by −1. In other words, the displacement (angle) of the right knee joint 56 is corrected, as shown in FIG. 67, and the displacement (angle) of the left knee joint 56 is corrected in reverse therefrom. Further, for the 34th compensating angle θ34, the displacement (angle) of the right hip joint 55 in the pitch direction is corrected in proportion to θ34 and also the displacement (angle) of the left hip joint 55 in the pitch direction is corrected at the same time by the amount obtained by multiplying the correction amount of the displacement of the right hip joint 55 in the pitch direction by −1. In other words, the displacement (angle) of the right hip joint 55 in the pitch direction is corrected, as shown in FIG. 68, and the displacement (angle) of the left hip joint 55 in the pitch direction is corrected in reverse therefrom. Incidentally, Q1 and Q1″ in FIG. 67 denote a desired floor reaction force central point of the foot 58 before the corrections of the joint displacements described above and a desired floor reaction force central point of the foot 58 after the corrections, respectively. Further, Q3 and Q3″ in FIG. 68 denote a desired floor reaction force central point of a knee before the corrections of the joint displacements described above and a desired floor reaction force central point of the knee after the corrections, respectively.

By correcting joint displacements as described above, the relative angle between the segment connecting a desired total floor reaction force central point (a desired 123456th node floor reaction force central point) and the overall center-of-gravity of the robot 51 (or the center-of-gravity of the body or the center-of-gravity of a plurality of portions, including the body 53) and the inclination angle of an n-th node (n=12,34,1234) corresponding to ground contact portions (a knee and the foot 58) is changed by the aforesaid converted node compensating angle θ12, θ23, and θ1234. Thus, an actual floor reaction force is faithfully controlled, leading to improved posture stability and ground contact properties of the robot 51.

Instead of directly correcting joint displacement commands (desired joint displacements) as described above, preferentially corrected desired ground contact portion positions/postures, which are the corrected desired positions/postures of the ground contact portions (specifically, the feet 58 and the hands 62) whose positions/postures are to be preferentially corrected to cause approximately the same desired joint displacements may be determined, and then, based on the determined preferentially corrected desired ground contact portion positions/postures, joint displacement commands may be determined by inverse kinematics calculation.

Because of insufficient degrees of freedom of joints, it is impossible to determine joint displacements by the inverse kinematics calculation to satisfy desired body position/posture and all corrected desired ground contact portion positions/postures; therefore, when carrying out the inverse kinematics calculation, some corrected desired ground contact portion positions/postures out of all corrected desired ground contact portion positions/postures are used. The some corrected desired ground contact portion positions/postures are referred to as the aforesaid preferentially corrected desired ground contact portion positions/postures.

For example, in the situation shown in FIG. 67, for the 1234th node compensating angle θ1234, the position obtained by rotationally moving a desired foot position about a knee may be determined as the preferentially corrected desired foot position.

Incidentally, in the sixth embodiment and the seventh embodiment, as an example, the compliance operation in the situation wherein the robot 51 is on its knees has been explained. The following will describe the adaptation to a compliance operation in a situation wherein the robot is sitting on a chair. This will be explained as an eighth embodiment.

Eighth Embodiment

FIG. 70 shows the construction of an essential section of a robot according to the present embodiment. This robot 71 is provided with floor reaction force sensors 73 and 73 for detecting floor reaction forces (load sensors, such as six-axis force sensors) at the right and left, respectively, of the base end surface of buttocks 72. Instead of providing the floor reaction force sensors 73 and 73 at the right and left, a single floor reaction force sensor that detects the resultant force of the forces applied to the right and left of the base end surface of the buttocks 72 may be provided.

In this case, the outsides of the floor reaction force sensors 73 and 73 are covered with a soft member (elastic member) 74, such as a sponge, as illustrated. To enhance the accuracy of the compliance control in the ground contact portions of the buttocks 72, it is desirable to shape the surface (ground contact surface) of the soft member (elastic member) 74 into a round convex surface in addition to covering them with the soft member. It is desirable to provide the member 74 such that the horizontal positions of the detection positions of the floor reaction force sensors 73 and 73 (the positions of the sensor main bodies) are aligned with the horizontal position of the apex of the convex surface of the aforesaid member 74 especially when the robot 71 is sitting on a chair through the intermediary of its buttocks 72. This arrangement reduces the nonlinearity of the relationship between a corrective operation of the position/posture of a ground contact portion of the robot 71 and a floor reaction force, resulting in better control performance of the compliance control of the robot 71.

In the robot 71, legs (link mechanisms) 52 and 55 are provided extendedly from the right and left sides of the buttocks 72. The structures of the legs 55 and 55, including their joints, are the same as those of, for example, the aforesaid sixth embodiment. Hence, the same reference marks as those related to the legs 5 and 55 in the sixth embodiment will be used, and the explanations thereof will be omitted. However, in the present embodiment, the knees of the legs 55 may not be provided with floor reaction force sensors.

Further, as shown in the figure, a torso (body) 77 is provided on the upper side of the buttocks 72, and arms 79 and 79 are provided extendedly from both sides of the upper portion of the torso 77 through the intermediary of shoulder joints 78 and 78. Incidentally, the arms 79 may have the same structure as that in the robot 51 of the aforesaid sixth embodiment. Further, the torso 77 is connected with the buttocks 72 through the intermediary of a joint 80. In this case, the joint 80 is constructed of a torso turning joint 80 a for turning the torso 77 in the yaw direction relative to the buttocks 72, and a torso flexing joint 80 b for tilting the torso 77 in the longitudinal and lateral directions relative to the buttocks 72. The joints provided in the robot 71 as described above are operated by actuators, which are not shown. Although not shown, a control device 50, which is similar to that in the aforesaid first embodiment and the like, is installed in the buttocks 72 or the torso 77.

In the present embodiment, the hierarchical structure may be set, as shown in FIG. 70, for the robot 71 sitting on a chair or the like through the intermediary of the buttocks 72. More specifically, a right foot 58, a left foot 58, the right portion of the base end surface of the buttocks 72 (the portion to which a right floor reaction force sensor 73 is attached), and the left portion of the base end surface of the buttocks 72 (the portion to which a left floor reaction force sensor 73 is attached) as ground contact portions are associated with a first node, a second node, a third node, and a fourth node, respectively, which are leaf nodes. Further, a 12th node having the first node and the second node as child nodes and a 34th node having the third node and a fourth node as child nodes have been set as intermediate nodes, and a 1234th node having the 12th node and the 34th node as child nodes has been set as a root node. Further, according to the policy similar to that in the aforesaid first embodiment and the like, the nodes are associated with desired floor reaction force central points Qn(n=1,2,3,4,12,34,1234), as illustrated. Incidentally, a desired floor reaction force central point Q1234 of the 1234th node, which is the root node, is identical to a desired total floor reaction force central point P.

In the present embodiment, the control device 50 is supposed to have a functional construction similar to that shown in the aforesaid FIG. 2.

In this case, a desired ground contact portion trajectory of a desired motion in a desired gait output by a gait generating device 100 in the present embodiment is constructed of the desired position/posture trajectory of each foot 58 and the desired position/posture trajectory of the buttocks 72. The body position/posture trajectory of the desired motion means the position/posture trajectory of the torso 77. Incidentally, the desired motion includes the position/posture trajectory of the distal portion of each arm 79. Further, the desired total floor reaction force central point P is on a virtual plane in the air rather than on an actual floor surface.

In the present embodiment, a hierarchical compliance operation determiner 114 has the same functional components as those of the aforesaid sixth embodiment (refer to FIG. 62). However, in the present embodiment, a compensating total floor reaction force moment distributor 114 a determines and outputs the node compensating floor reaction force moments of the intermediate nodes and the root node in the hierarchical structure shown in FIG. 70 and the node compensating floor reaction force moments of the leaf nodes corresponding to the feet 58. Further, a compensating angle determiner determines and outputs the node compensating angles of the intermediate nodes and the root node in the hierarchical structure shown in FIG. 70 and the node compensating angles of the leaf nodes corresponding to the feet 58. In this case, the basic techniques for determining these node compensating floor reaction force moments and node compensating angles may be the same as the techniques explained in the first to the third embodiments or the sixth embodiment. Further, a floor configuration estimator determines estimated n-th floor height errors Zfn_estm(n=1,2,3,4) by the same technique as the technique explained in the aforesaid third embodiment, as in the case of the aforesaid sixth embodiment, and also estimates ground contact portion floor inclination errors θfn(n=1,2) of the feet 58.

Further, a robot geometric model (inverse kinematics calculator), which is a functional component of the control device 50, corrects the positions/postures of the ground contact portion of the buttocks 72 and the feet 58 (ground contact portions) to restrain the occurrence of slippage, such as twist, of the ground contact portions and also corrects the position/posture of the body (torso) 77 basically on the basis of the corrected desired ground contact portion positions/postures with deformation compensation and the desired body position/posture mentioned above by the same technique as that in the aforesaid sixth embodiment.

The processing of the control device 50 other than that explained above may be the same as that of the aforesaid sixth embodiment.

Several modifications related to the embodiments explained above will now be explained.

An actual node floor reaction force that cannot be directly detected by a floor reaction force sensor may be estimated by an observer by using another actual ground contact portion floor reaction force directly detected by a floor reaction force sensor, a detected value of an acceleration sensor, a detected value of a clinometer, and the like, or it may be estimated by a simplified algebraic relationship. For instance, the hierarchical compliance operation is a rotational compliance operation in which a ground contact portion is rotated about a desired total floor reaction force central point; therefore, even when the compliance operation is performed, the vertical acceleration of the overall center-of-gravity of the robot (or an acceleration component in the direction of a segment that connects a desired total floor reaction force central point and the overall center-of-gravity) substantially agrees with the vertical acceleration of the overall center-of-gravity in a desired gait (desired motion) of the robot (or an acceleration component in the direction of a segment that connects a desired total floor reaction force central point and the overall center-of-gravity). Hence, the sum of all actual ground contact portion floor reaction force vertical components substantially agrees with the value obtained by multiplying the sum of the vertical acceleration of the overall center-of-gravity in the desired gait (desired motion) of the robot and the gravitational acceleration by the total mass of the robot.

Thus, if an actual n-th node floor reaction force cannot be directly detected, then first, the sum (hereinafter referred to as the actual non-n-th node floor reaction force) of the actual floor reaction forces (actual ground contact portion floor reaction forces) of all leaf nodes that do not have an n-th node as their ancestor node and that are not the n-th node itself is determined.

Next, an estimated n-th node floor reaction force, which is an estimated value of an actual n-th node floor reaction force, is determined according to the following expression 57. $\begin{matrix} {{{Estimated}\quad n\text{-}{th}\quad{node}\quad{floor}\quad{reaction}\quad{force}} = {{\left( {{{Overall}\quad{center}\text{-}{of}\text{-}{gravity}\quad{acceleration}\quad{of}\quad{desired}\quad{gait}} + {{Gravitational}\quad{acceleration}}} \right)*{Total}\quad{mass}} - {{Actual}\quad n\text{-}{th}\quad{node}\quad{floor}\quad{reaction}\quad{force}}}} & {{Expression}\quad 57} \end{matrix}$

If the body or the like is provided with an acceleration sensor, then the center-of-gravity acceleration of the entire robot (hereinafter referred to as an estimated overall center-of-gravity acceleration) may be estimated on the basis of a detected value of the acceleration sensor and at least one of a desired gait and an actual joint displacement, and an estimated n-th node floor reaction force, which is the estimated value of an actual n-th node floor reaction force, may be determined according to the following expression 58. $\begin{matrix} {{{Estimated}\quad n\text{-}{th}\quad{node}\quad{floor}\quad{reaction}\quad{force}} = {{\left( {{{Estimated}\quad{overall}\quad{centerb}\text{-}{of}\text{-}{gravity}\quad{acceleration}} + {{Gravitational}\quad{acceleration}}} \right)*{Total}\quad{mass}} - {{Actual}\quad n\text{-}{th}\quad{node}\quad{floor}\quad{reaction}\quad{forces}}}} & {{Expression}\quad 58} \end{matrix}$

For example, in the aforesaid sixth embodiment, if no floor reaction force sensors for detecting knee floor reaction forces are provided or floor reaction force sensors for detecting knee floor reaction forces fail, then an estimated 34th node floor reaction force is determined according to the following expression 59. $\begin{matrix} {{{Estimated}\quad 34{th}\quad{node}\quad{floor}\quad{reaction}\quad{force}} = {{{\left( {{{Overall}\quad{center}\text{-}{of}\text{-}{gravity}\quad{acceleration}\quad{of}\quad{desired}\quad{gait}}\quad + {{Center}\text{-}{of}\text{-}{gravity}\quad{acceleration}}} \right)*{Total}\quad{mass}} - {{Actual}\quad 34{th}\quad{node}\quad{floor}\quad{reaction}\quad{forces}}} = {{{Overall}\quad{center}\text{-}{of}\text{-}{gravity}\quad{acceleration}\quad{of}\quad{desired}\quad{gait}*{Total}\quad{mass}} - \left( {{{Actual}\quad 12{th}\quad{node}\quad{floor}\quad{reaction}\quad{force}} + {{Actual}\quad 56{th}\quad{node}\quad{floor}\quad{reaction}\quad{force}}} \right)}}} & {{Expression}\quad 59} \end{matrix}$

Further, the estimated 34th node floor reaction force is used in place of an actual 34th node floor reaction force to determine the 1234th node compensating angle θ1234 by the compliance operation processing, and an estimated 1234th node relative floor height error Z1234rel_estm is also estimated by floor configuration estimation processing.

In determining the 34th node compensating angle θ34, it is assumed that the floor is as expected, and an estimated third node floor reaction force, which is the estimated value of an actual third node floor reaction force, and an estimated fourth node floor reaction force, which is the estimated value of an actual fourth node floor reaction force, are determined on the basis of the corrected desired third ground contact portion position/posture with deformation compensation, the corrected desired fourth ground contact portion position/posture with deformation compensation, and an estimated 34th node floor reaction force.

Further, the estimated third node floor reaction force and the estimated fourth node floor reaction force are used in place of an actual third node floor reaction force and an actual fourth node floor reaction force to determine the 34th node compensating angle θ34 by the compliance operation processing.

Instead of determining the corrected desired node floor reaction force moment about a desired node floor reaction force central point on the basis of the required moment for restoring the posture of the robot (compensating total floor reaction force moment) so as to be equivalent to adding the required moment for restoration as described above, a corrected desired node floor reaction force central point obtained by correcting a desired node floor reaction force central point may be determined on the basis of the required moment for restoration (compensating total floor reaction force moment). In this case, a desired node floor reaction force moment about a desired node floor reaction force central point is not corrected, so that it remains to be zero.

Further, in order to compensate for a component that could not be controlled within an n-th node floor reaction force (an expected or detected control error component), the desired floor reaction force of the parent node of the n-th node may be corrected. More specifically, the value of the difference between an actual n-th node floor reaction force and a desired n-th node floor reaction force in the last control cycle or the value obtained by passing the aforesaid difference through a low-pass filter may be added to the desired floor reaction force of the n-th node.

In the processing of estimating a floor configuration, a tree structure that is different from the tree structure for the compliance control may be set. The tree structure may have two layers composed of a root node and leaf nodes. Further, the weights of nodes may be set to be different from those for the compliance control. A node may be set to any weight as long as the weight of the node is zero in a period during which a node floor reaction force is zero (a period during which all ground contact portions belonging to the node or the ground contact portion corresponding to the node moves in the air).

Further, in the processing of estimating a floor configuration, each corrected node weight may be determined on the basis of the aforesaid corrected desired node floor reaction force central point, and a vector having the determined corrected node weight as an element may be used as the weight for estimating the floor configuration.

When determining an estimated relative floor height error of each point (each ground contact portion) in the processing of estimating a floor configuration, instead of using an actual floor reaction force, a mechanism compliance model, and the detected value of a posture sensor, a deformation amount detector for detecting the deformation amount of a compliance mechanism or the like may be provided so as to use a detected value of the deformation amount detector and a detected value of a posture sensor.

If a floor configuration is to be not only relatively estimated but globally estimated also (estimating a floor configuration on a global coordinate system), then the following may be carried out.

1) Estimating the global height of a floor in a case where the height of the body of a robot on the global coordinate system (hereinafter referred to as the global height) can be detected:

A detector that detects the global height of a predetermined portion, such as a body, of a robot (a body height estimator using an acceleration sensor (e.g., the estimator disclosed in PCT/JP03/05448 by the present applicant) or an external sensor, such as a visual sensor) is used. The value of an estimated floor height error on the global coordinate system is determined on the basis of the estimated body global height, a joint displacement (an actual joint displacement or a desired joint displacement), the detected value of an actual body posture inclination, and the detected value of an actual floor reaction force. Thus, the estimated floor height error of the root node will have a meaning as the weighted average value of the global estimated floor height error of every ground contact portion.

2) Estimating the global height of a floor in a situation wherein the global height of the body is unknown:

If the global value of a certain estimated n-th floor height error (the error in the global coordinate system rather than the relative value in a group) is known, then the global values of the estimated floor height errors of all points (ground contact portions) are determined on the basis of the above known global value and estimated relative floor height errors. If the global value of the estimated n-th floor height error at the start point of a travel of a robot is known, then the global value of the floor height error of a ground contact portion that newly comes in contact with the ground will be sequentially determined as the robot continues to travel; therefore, a map of floor heights on a travel trajectory of the robot can be created by storing the above sequentially determined global values.

Further, in the processing of estimating a floor configuration, each corrected node weight (obtained by correcting the value of the weight of each node) may be determined on the basis of the aforesaid corrected desired node floor reaction force central point, and the determined corrected node weight may be used as the weight for defining an actual n-th node relative floor height or the like in estimating the floor configuration. Further, in estimating a floor configuration, the weight used to define the actual n-th node relative floor height or the like does not necessarily have to be the same as the aforesaid weight determined by the desired floor reaction force distributor.

Supplementally, the weight for defining the actual n-th node relative floor height or the like does not necessarily have to agree with the weight determined by the desired floor reaction force distributor; however, if they are to agree with each other (in this case, the desired floor reaction force central point of the root node will automatically agree with a desired total floor reaction force central point), then the influence of a floor configuration error can be canceled simply by adding an n-th node estimated floor inclination error to an n-th node compensating angle (n=1,2, . . . ) for the compliance operation, thus making it possible to reduce the calculation volume for canceling the influence of the floor configuration error.

Furthermore, estimated floor configuration errors may be stored as the map information of a floor configuration, and a desired gait may be generated on the basis of the stored map information of the floor configuration when a robot walks in the same place next time.

Incidentally, the arithmetic processing sequences in the block diagrams or the flowcharts showing the arithmetic processing functions in the embodiments described above may be subjected to equivalent modifications.

Further, the present invention can be applied not only to a mobile robot but also a wheel-type mobile body having an active suspension.

INDUSTRIAL APPLICABILITY

As described above, the present invention is useful as the one that makes it possible to provide a mobile body, such as a legged mobile robot, that permits highly stable, smooth operations to be achieved by properly controlling floor reaction forces. 

1. A control device for a mobile body comprising desired motion determining means for determining a desired motion of the mobile body, such as a mobile robot, having three or more ground contact portions connected through the intermediary of a plurality of joints such that mutual relative heights can be manipulated, and actual posture inclination detecting means for detecting or estimating the actual posture inclination of a predetermined portion, such as a base body of the mobile body, the operation of the mobile body being controlled to make the motion of the mobile body follow a desired motion, further comprising: node operation controlling means that categorizes the ground contact portions in a tree structure manner such that all of the ground contact portions become leaf nodes, and an intermediate node exists between the leaf nodes and a root node having all the leaf nodes as descendant nodes, determines, on each of C-th nodes, which is a node having a plurality of ground contact portions as descendant nodes, the correction amounts of desired relative heights of a plurality of ground contact portions belonging to the C-th node for each of the C-th nodes such that a posture inclination error is approximated to zero on the basis of at least the posture inclination error, which is the difference between the actual posture inclination and a desired posture inclination of the predetermined part in the desired motion, determines, on the basis of a correction amount obtained by combining the determined correction amounts of all the C-th nodes, a corrected desired motion by adding at least a first correction of the desired relative heights of the plurality of ground contact portions belonging to each C-th node to the desired motion, and operates the joints such that the corrected desired motion that has been determined is satisfied.
 2. The control device for a mobile body according to claim 1, wherein if each node in the tree structure has a set weight, and regarding a B-th node, which is each node in the tree structure, if the B-th node is the leaf node, then the height of a ground contact portion, which is the leaf node, is defined as the height of the B-th node, and if the B-th node has child nodes, then a weighted mean value, which uses the weight, of the heights of all child nodes of the B-th node is defined as the height of the B-th node, and if the node operation controlling means determines the corrected desired motion for only one arbitrary node among the C-th nodes by adding a first correction of the desired relative heights of the plurality of ground contact portions belonging to that one node to the desired motion, then the desired height of that one node in the corrected desired motion is maintained at a desired height in the desired motion.
 3. A control device for a mobile body comprising desired motion determining means for determining a desired motion of the mobile body, such as a mobile robot, having three or more ground contact portions connected through the intermediary of a plurality of joints such that mutual relative heights can be manipulated, and actual posture inclination detecting means for detecting or estimating the actual posture inclination of a predetermined portion, such as a base body of the mobile body, the operation of the mobile body being controlled to make the motion of the mobile body follow a desired motion, a weight being set for each node in a tree structure when the ground contact portions are categorized in the tree structure manner such that all of the ground contact portions become leaf nodes and an intermediate node exists between the leaf nodes and a root node having all the leaf nodes as descendant nodes, the control device comprising a node operation controlling means that determines a corrected desired motion obtained by adding at least a first correction of desired relative heights of the plurality of ground contact portions belonging to a C-th node to the desired motion such that a posture inclination error is approximated to zero on the basis of at least the posture inclination error, which is the difference between the actual posture inclination and a desired posture inclination of the predetermined portion in the desired motion, and operates the joints such that the corrected desired motion that has been determined is satisfied, wherein, on a B-th node, which is each node in the tree structure, if the B-th node is the leaf node, then the height of the ground contact portion, which is the leaf node, is defined as the height of the B-th node, and if the B-th node has child nodes, then a weighted mean value, which uses the weight, of the heights of all child nodes of the B-th node, is defined as the height of the B-th node, and if the node operation controlling means determines the desired motion for only the C-th node by adding a first correction of the desired relative heights of the plurality of ground contact portions belonging to the C-th node to the desired motion, then the desired height of the C-th node in the corrected desired motion is maintained at a desired height in the desired motion.
 4. The control device for a mobile body according to claim 2, wherein, on each node having child nodes in the tree structure, the weights of all child nodes of the each node are set such that the total sum of the weights of all the child nodes becomes one.
 5. The control device for a mobile body according to claim 2, comprising means for variably setting the weight of the each node, wherein if leaf nodes corresponding to ground contact portions, respectively, that are not in contact with the ground are defined as D-th nodes, then the means for setting the weights sets the weight of each D-node or the weight of at least one of ancestor nodes of the D-th node to zero.
 6. The control device for a mobile body according to claim 1, wherein the ground contact portions are connected to the base body of the mobile body in such a manner that they are movable with respect to the base body, and when determining the corrected desired motion, the node operation controlling means determines the corrected desired motion by further adding to the desired motion a correction of a desired posture of the base body to restrain slippage, such as twist, of the ground contact portions belonging to the C-th node on a floor surface.
 7. The control device for a mobile body according to claim 3, wherein the ground contact portions are connected to the base body of the mobile body in such a manner that they are movable with respect to the base body, and when determining the corrected desired motion, the node operation controlling means determines the corrected desired motion by further adding to the desired motion a correction of a desired posture of the base body to restrain slippage, such as twist, of the ground contact portions belonging to the C-th node on a floor surface.
 8. The control device for a mobile body according to claim 6, wherein the node operation controlling means determines the corrected desired motion such that the horizontal position of the overall center-of-gravity of the mobile body in the corrected desired motion or the horizontal position of a predetermined representative point of the base body of the mobile body is substantially the same as the horizontal position in the desired motion.
 9. The control device for a mobile body according to claim 6, wherein the mobile body is a robot comprising a plurality of link mechanisms extended from its base body, and at least one link mechanism out of the plurality of link mechanisms has a joint provided at an intermediate portion between at least its distal portion and the end portion adjacent to the base body, the intermediate portion being the ground contact portion.
 10. The control device for a mobile body according to claim 7, wherein the mobile body is a robot comprising a plurality of link mechanisms extended from its base body, and at least one link mechanism out of the plurality of link mechanisms has a joint provided at an intermediate portion between at least its distal portion and the end portion adjacent to the base body, the intermediate portion being a ground contact portion belonging to the C-th node.
 11. The control device for a mobile body according to claim 9, wherein the intermediate portion is provided with an elastic member that resiliently deforms when it comes in contact with the ground.
 12. The control device for a mobile body according to claim 6, wherein the mobile body comprises a plurality of link mechanisms which are extended from its base body and each of which has one or more joints, and at least one link mechanism among the plurality of link mechanisms and the base body are provided with the ground contact portions.
 13. The control device for a mobile body according to claim 7, wherein the mobile body comprises a plurality of link mechanisms which are extended from its base body and each of which has one or more joints, at least one link mechanism among the plurality of link mechanisms and the base body are provided with the ground contact portions, and ground contact portions belonging to the C-th node include at least the ground contact portions of the base body.
 14. The control device for a mobile body according to claim 12, wherein at least one ground contact portion of the base body is provided with an elastic member that resiliently deforms when it comes in contact with the ground.
 15. The control device for a mobile body according to claim 1, the ground contact portions being connected to the base body through the intermediary of connecting mechanisms such that they can be moved with respect to the base body of the mobile body, comprising: means for determining a translational force component of a desired floor reaction force to be applied to ground contact portions belonging to the C-th node when the joint is operated such that the corrected desired motion is satisfied, wherein the node operation controlling means comprises a means for estimating a deformation amount that occurs at the connecting mechanisms between the base body and the ground contact portions belonging to the C-th node and the ground contact portions when the translational force component of the desired floor reaction force is applied to the ground contact portions belonging to the C-th node on the basis of the translational force components of the desired floor reaction forces of the ground contact portions belonging to at least the C-th node, and determines the corrected desired motion by further adding a second correction of the desired heights of the plurality of ground contact portions belonging to the C-th node, which is for canceling the estimated deformation amount, to the desired motion.
 16. The control device for a mobile body according to claim 1, comprising: floor configuration estimating means for estimating a parameter that specifies the relative heights of the ground contact surfaces of a plurality of ground contact portions belonging to at least the C-th node as the floor configuration parameter that represents a floor configuration, wherein the node operation controlling means determines the corrected desired motion by further adding a third correction of the desired relative heights of a plurality of the ground contact portions belonging to the C-th node on the basis of an estimated value of a floor configuration parameter when determining the corrected desired motion.
 17. The control device for a mobile body according to claim 16, comprising: floor reaction force detecting means for detecting or estimating an actual floor reaction force that acts on each of the ground contact portions, wherein the floor configuration estimating means performs estimation while sequentially updating the floor configuration parameter on the basis of a past value of an estimated value of the floor configuration parameter, a difference between the corrected desired motion and the desired motion, the corrected motion, at least either a detected value or an estimated value of an actual joint displacement, which is a displacement amount of each joint of the mobile body, the actual posture inclination, and an actual floor reaction force of each of the ground contact portions belonging to at least the C-th node.
 18. The control device for a mobile body according to claim 16, comprising: floor reaction force detecting means for detecting or estimating an actual floor reaction force that acts on each of the ground contact portions; and means for sequentially determining at least an actual node floor reaction force, which is an actual value of the node floor reaction force of each child node of the C-th node, from the actual floor reaction force of each ground contact portion belonging to the C-th node if, regarding A-th nodes, which are the nodes in the tree structure, the A-th nodes are the leaf nodes, then the floor reaction forces acting on the ground contact portions, which are the leaf nodes, are defined as the node floor reaction forces of the A-th nodes, and if the A-th nodes have child nodes, then the resultant force of the node floor reaction forces of all the child nodes of the A-th nodes is defined as the node floor reaction force of the A-th nodes, wherein the floor configuration estimating means performs estimation while sequentially updating the floor configuration parameter on the basis of the past value of an estimated value of the floor configuration parameter, the difference between the corrected desired motion and the desired motion, the corrected motion, at least either a detected value or an estimated value of an actual joint displacement, which is a displacement amount of each joint of the mobile body, the actual posture inclination, and a relative relationship among the actual node floor reaction forces of the child nodes of at least the C-th node.
 19. The control device for a mobile body according to claim 16, the ground contact portions being connected to the base body through the intermediary of connecting mechanisms such that they are movable with respect to the base body of the mobile body, comprising: deformation amount detecting means for detecting or estimating the amounts of deformations that occur at the connecting mechanisms between the base body and the ground contact portions and at the ground contact portions, wherein the floor configuration estimating means performs estimation while sequentially updating the floor configuration parameter on the basis of the past value of an estimated value of the floor configuration parameter, a difference between the corrected desired motion and the desired motion, the corrected motion, at least either a detected value or an estimated value of an actual joint displacement, which is a displacement amount of each joint of the mobile body, the actual posture inclination, and the deformation amount associated with each ground contact portion belonging to at least the C-th node.
 20. The control device for a mobile body according to claim 19, comprising a floor reaction force detecting means for detecting or estimating an actual floor reaction force acting on each of the ground contact portions belonging to the C-th node, wherein the deformation amount detecting means estimates the deformation amount on the basis of the actual floor reaction force.
 21. The control device for a mobile body according to claim 16, wherein, each node in the tree structure has a set weight, and regarding a B-th node, which is each node in the tree structure, if the B-th node is the leaf node, then the height of the ground contact surface of the ground contact portion, which is the leaf node, is defined as the height of the ground contact surface of the B-th node, and if the B-th node has child nodes, then a weighted mean value, which uses the weight, of the heights of the ground contact surfaces of all child nodes of the B-th node is defined as the height of the ground contact surface of the B-th node, and when these definitions apply, the floor configuration parameter estimated by the floor configuration estimating means is a parameter that specifies a relative relationship of the heights of the ground contact surfaces of a plurality of child nodes of the C-th node.
 22. The control device for a mobile body according to claim 21, wherein, if at least one of the child nodes of the C-th node is about to float, then the floor configuration estimating means estimates the floor configuration parameter that specifies the relative relationship of the heights of the ground contact surfaces of the plurality of child nodes while retaining, at a fixed value, the relative height of the ground contact surface of the child node that is about to float.
 23. The control device for a mobile body according to claim 16, wherein the floor configuration estimating means estimates the floor configuration parameter by using a low-pass filter to alleviate fluctuation in the floor configuration parameter.
 24. The control device for a mobile body according to claim 16, wherein a weight for estimating a floor configuration is set on each node of the tree structure, and if processing in which, when a predetermined type of state amount, such as a height or a floor reaction force, is associated with each leaf node in the tree structure, the state amount of each node having child nodes is defined as a weighted mean value, which uses the weight, of the state amounts of all child nodes of the node, a value obtained by subtracting a state amount of a parent node of each node from the state amount of the node is determined as a node relative state amount of the node on the node except a root node, and zero is determined as a relative state amount of the root node, is defined as the processing for relatively hierarchizing the predetermined type of state amount on each node, if a node relative floor reaction force Fn_rel(n=1,2, . . . ) hierarchically relativized on each node is determined on the basis of a floor reaction force Fn(n=1,2, . . . ) acting on each of the ground contact portions, which are the leaf nodes, and when a vector (Fa1_rel,Fa2_rel, . . . ,Far_rel) having the node relative floor reaction forces of all child nodes aj(j=1,2, . . . ,r. r denotes the total number of the child nodes of an n-th node) of an n-th node as its elements, which is an arbitrary node having child nodes, is represented by a linear linkage of a plurality of predetermined mutually independent vectors R(j)(j=1,2, . . . ,r−1) that are all orthogonal to a vector (Wa1,Wa2, . . . ,War) having the weights of all child nodes of the n-th node as its elements, a vector having a coefficient of the linear linkage as its element is defined as a node expansion floor reaction force moment Mn_exp of the n-th node, and if a node relative height Zn_rel(n=1,2, . . . ) hierarchically relativized on each node is determined on the basis of a height Zn(n=1,2, . . . ) of the ground contact surface of each of the ground contact portions, which are the leaf nodes, and when a vector (Za1_rel, Za2_rel, . . . ,Zar_rel) having the node relative heights of all child nodes aj(j=1,2, . . . ,r. r denotes the total number of the child nodes of an n-th node) of the n-th node as its elements is expressed by a linear linkage of the plurality of the predetermined mutually independent vectors R(j)(j=1,2, . . . ,r−1), a vector having a coefficient of the linear linkage as its element is defined as a node expansion inclination angle θn of the n-th node, then, the floor configuration estimating means estimates the floor configuration parameter by using the node expansion floor reaction force moment Mn_exp(n=C) or the node expansion inclination angle θn(n=C) of at least the C-th node.
 25. The control device for a mobile body according to claim 24, wherein, regarding a B-th node, which is each node in the tree structure, if the B-th node is the leaf node, then the height of the ground contact surface of the ground contact portion, which is the leaf node, is defined as the height of the ground contact surface of the B-th node, and if the B-th node has child nodes, then a weighted mean value, which uses the weight, of the heights of the ground contact surfaces of all child nodes of the B-th node, is defined as the height of the ground contact surface of the B-th node, then the floor configuration parameter includes a parameter that uses the node expansion inclination angle to indicate the relative height of the ground contact surface of each child node of the C-th node.
 26. The control device for a mobile body according to claim 24, comprising a floor reaction force detecting means for detecting or estimating an actual floor reaction force acting on each of the ground contact portions, wherein the node operation controlling means comprises means for sequentially determining the node expansion moment on the basis of at least an actual floor reaction force of each of the ground contact portions, and a means for sequentially determining the node expansion inclination angle by multiplying the determined node expansion moment by a predetermined matrix, and estimates a new floor configuration parameter on the basis of the determined node expansion inclination angle and the past value of an estimated value of the floor configuration parameter.
 27. The control device for a mobile body according to claim 3, wherein, on each node having child nodes in the tree structure, the weights of all child nodes of the each node are set such that the total sum of the weights of all the child nodes becomes one.
 28. The control device for a mobile body according to claim 3, comprising means for variably setting the weight of the each node, wherein if leaf nodes corresponding to ground contact portions, respectively, that are not in contact with the ground are defined as D-th nodes, then the means for setting the weights sets the weight of each D-node or the weight of at least one of ancestor nodes of the D-th node to zero.
 29. The control device for a mobile body according to claim 2, wherein the ground contact portions are connected to the base body of the mobile body in such a manner that they are movable with respect to the base body, and when determining the corrected desired motion, the node operation controlling means determines the corrected desired motion by further adding to the desired motion a correction of a desired posture of the base body to restrain slippage, such as twist, of the ground contact portions belonging to the C-th node on a floor surface.
 30. The control device for a mobile body according to claim 29, wherein the node operation controlling means determines the corrected desired motion such that the horizontal position of the overall center-of-gravity of the mobile body in the corrected desired motion or the horizontal position of a predetermined representative point of the base body of the mobile body is substantially the same as the horizontal position in the desired motion.
 31. The control device for a mobile body according to claim 29, wherein the mobile body is a robot comprising a plurality of link mechanisms extended from its base body, and at least one link mechanism out of the plurality of link mechanisms has a joint provided at an intermediate portion between at least its distal portion and the end portion adjacent to the base body, the intermediate portion being the ground contact portion.
 32. The control device for a mobile body according to claim 31, wherein the intermediate portion is provided with an elastic member that resiliently deforms when it comes in contact with the ground.
 33. The control device for a mobile body according to claim 29, wherein the mobile body comprises a plurality of link mechanisms which are extended from its base body and each of which has one or more joints, and at least one link mechanism among the plurality of link mechanisms and the base body are provided with the ground contact portions.
 34. The control device for a mobile body according to claim 33, wherein at least one ground contact portion of the base body is provided with an elastic member that resiliently deforms when it comes in contact with the ground.
 35. The control device for a mobile body according to claim 7, wherein the node operation controlling means determines the corrected desired motion such that the horizontal position of the overall center-of-gravity of the mobile body in the corrected desired motion or the horizontal position of a predetermined representative point of the base body of the mobile body is substantially the same as the horizontal position in the desired motion.
 36. The control device for a mobile body according to claim 10, wherein the intermediate portion is provided with an elastic member that resiliently deforms when it comes in contact with the ground.
 37. The control device for a mobile body according to claim 13, wherein at least one ground contact portion of the base body is provided with an elastic member that resiliently deforms when it comes in contact with the ground.
 38. The control device for a mobile body according to claim 3, the ground contact portions being connected to the base body through the intermediary of connecting mechanisms such that they can be moved with respect to the base body of the mobile body, comprising: means for determining a translational force component of a desired floor reaction force to be applied to ground contact portions belonging to the C-th node when the joint is operated such that the corrected desired motion is satisfied, wherein the node operation controlling means comprises a means for estimating a deformation amount that occurs at the connecting mechanisms between the base body and the ground contact portions belonging to the C-th node and the ground contact portions when the translational force component of the desired floor reaction force is applied to the ground contact portions belonging to the C-th node on the basis of the translational force components of the desired floor reaction forces of the ground contact portions belonging to at least the C-th node, and determines the corrected desired motion by further adding a second correction of the desired heights of the plurality of ground contact portions belonging to the C-th node, which is for canceling the estimated deformation amount, to the desired motion.
 39. The control device for a mobile body according to claim 3, comprising: floor configuration estimating means for estimating a parameter that specifies the relative heights of the ground contact surfaces of a plurality of ground contact portions belonging to at least the C-th node as the floor configuration parameter that represents a floor configuration, wherein the node operation controlling means determines the corrected desired motion by further adding a third correction of the desired relative heights of a plurality of the ground contact portions belonging to the C-th node on the basis of an estimated value of a floor configuration parameter when determining the corrected desired motion.
 40. The control device for a mobile body according to claim 39, comprising: floor reaction force detecting means for detecting or estimating an actual floor reaction force that acts on each of the ground contact portions, wherein the floor configuration estimating means performs estimation while sequentially updating the floor configuration parameter on the basis of a past value of an estimated value of the floor configuration parameter, a difference between the corrected desired motion and the desired motion, the corrected motion, at least either a detected value or an estimated value of an actual joint displacement, which is a displacement amount of each joint of the mobile body, the actual posture inclination, and an actual floor reaction force of each of the ground contact portions belonging to at least the C-th node.
 41. The control device for a mobile body according to claim 39, comprising: floor reaction force detecting means for detecting or estimating an actual floor reaction force that acts on each of the ground contact portions; and means for sequentially determining at least an actual node floor reaction force, which is an actual value of the node floor reaction force of each child node of the C-th node, from the actual floor reaction force of each ground contact portion belonging to the C-th node if, regarding A-th nodes, which are the nodes in the tree structure, the A-th nodes are the leaf nodes, then the floor reaction forces acting on the ground contact portions, which are the leaf nodes, are defined as the node floor reaction forces of the A-th nodes, and if the A-th nodes have child nodes, then the resultant force of the node floor reaction forces of all the child nodes of the A-th nodes is defined as the node floor reaction force of the A-th nodes, wherein the floor configuration estimating means performs estimation while sequentially updating the floor configuration parameter on the basis of the past value of an estimated value of the floor configuration parameter, the difference between the corrected desired motion and the desired motion, the corrected motion, at least either a detected value or an estimated value of an actual joint displacement, which is a displacement amount of each joint of the mobile body, the actual posture inclination, and a relative relationship among the actual node floor reaction forces of the child nodes of at least the C-th node.
 42. The control device for a mobile body according to claim 39, the ground contact portions being connected to the base body through the intermediary of connecting mechanisms such that they are movable with respect to the base body of the mobile body, comprising: deformation amount detecting means for detecting or estimating the amounts of deformations that occur at the connecting mechanisms between the base body and the ground contact portions and at the ground contact portions, wherein the floor configuration estimating means performs estimation while sequentially updating the floor configuration parameter on the basis of the past value of an estimated value of the floor configuration parameter, a difference between the corrected desired motion and the desired motion, the corrected motion, at least either a detected value or an estimated value of an actual joint displacement, which is a displacement amount of each joint of the mobile body, the actual posture inclination, and the deformation amount associated with each ground contact portion belonging to at least the C-th node.
 43. The control device for a mobile body according to claim 42, comprising a floor reaction force detecting means for detecting or estimating an actual floor reaction force acting on each of the ground contact portions belonging to the C-th node, wherein the deformation amount detecting means estimates the deformation amount on the basis of the actual floor reaction force.
 44. The control device for a mobile body according to claim 39, wherein, each node in the tree structure has a set weight, and regarding a B-th node, which is each node in the tree structure, if the B-th node is the leaf node, then the height of the ground contact surface of the ground contact portion, which is the leaf node, is defined as the height of the ground contact surface of the B-th node, and if the B-th node has child nodes, then a weighted mean value, which uses the weight, of the heights of the ground contact surfaces of all child nodes of the B-th node is defined as the height of the ground contact surface of the B-th node, and when these definitions apply, the floor configuration parameter estimated by the floor configuration estimating means is a parameter that specifies a relative relationship of the heights of the ground contact surfaces of a plurality of child nodes of the C-th node.
 45. The control device for a mobile body according to claim 44, wherein, if at least one of the child nodes of the C-th node is about to float, then the floor configuration estimating means estimates the floor configuration parameter that specifies the relative relationship of the heights of the ground contact surfaces of the plurality of child nodes while retaining, at a fixed value, the relative height of the ground contact surface of the child node that is about to float.
 46. The control device for a mobile body according to claim 39, wherein the floor configuration estimating means estimates the floor configuration parameter by using a low-pass filter to alleviate fluctuation in the floor configuration parameter.
 47. The control device for a mobile body according to claim 39, wherein a weight for estimating a floor configuration is set on each node of the tree structure, and if processing in which, when a predetermined type of state amount, such as a height or a floor reaction force, is associated with each leaf node in the tree structure, the state amount of each node having child nodes is defined as a weighted mean value, which uses the weight, of the state amounts of all child nodes of the node, a value obtained by subtracting a state amount of a parent node of each node from the state amount of the node is determined as a node relative state amount of the node on the node except a root node, and zero is determined as a relative state amount of the root node, is defined as the processing for relatively hierarchizing the predetermined type of state amount on each node, if a node relative floor reaction force Fn_rel(n=1,2, . . . ) hierarchically relativized on each node is determined on the basis of a floor reaction force Fn(n=1,2, . . . ) acting on each of the ground contact portions, which are the leaf nodes, and when a vector (Fa1_rel,Fa2_rel, . . . ,Far_rel) having the node relative floor reaction forces of all child nodes aj(j=1,2, . . . ,r. r denotes the total number of the child nodes of an n-th node) of an n-th node as its elements, which is an arbitrary node having child nodes, is represented by a linear linkage of a plurality of predetermined mutually independent vectors R(j)(j=1,2, . . . ,r−1) that are all orthogonal to a vector (Wa1,Wa2, . . . ,War) having the weights of all child nodes of the n-th node as its elements, a vector having a coefficient of the linear linkage as its element is defined as a node expansion floor reaction force moment Mn_exp of the n-th node, and if a node relative height Zn_rel(n=1,2, . . . ) hierarchically relativized on each node is determined on the basis of a height Zn(n=1,2, . . . ) of the ground contact surface of each of the ground contact portions, which are the leaf nodes, and when a vector (Za1_rel, Za2_rel, . . . ,Zar_rel) having the node relative heights of all child nodes aj(j=1,2, . . . ,r. r denotes the total number of the child nodes of an n-th node) of the n-th node as its elements is expressed by a linear linkage of the plurality of the predetermined mutually independent vectors R(j)(j=1,2, . . . ,r−1), a vector having a coefficient of the linear linkage as its element is defined as a node expansion inclination angle θn of the n-th node, then, the floor configuration estimating means estimates the floor configuration parameter by using the node expansion floor reaction force moment Mn_exp(n=C) or the node expansion inclination angle θn(n=C) of at least the C-th node.
 48. The control device for a mobile body according to claim 47, wherein, regarding a B-th node, which is each node in the tree structure, if the B-th node is the leaf node, then the height of the ground contact surface of the ground contact portion, which is the leaf node, is defined as the height of the ground contact surface of the B-th node, and if the B-th node has child nodes, then a weighted mean value, which uses the weight, of the heights of the ground contact surfaces of all child nodes of the B-th node, is defined as the height of the ground contact surface of the B-th node, then the floor configuration parameter includes a parameter that uses the node expansion inclination angle to indicate the relative height of the ground contact surface of each child node of the C-th node.
 49. The control device for a mobile body according to claim 47, comprising a floor reaction force detecting means for detecting or estimating an actual floor reaction force acting on each of the ground contact portions, wherein the node operation controlling means comprises means for sequentially determining the node expansion moment on the basis of at least an actual floor reaction force of each of the ground contact portions, and a means for sequentially determining the node expansion inclination angle by multiplying the determined node expansion moment by a predetermined matrix, and estimates a new floor configuration parameter on the basis of the determined node expansion inclination angle and the past value of an estimated value of the floor configuration parameter. 